/* R-Dev-Ino dual Motor Controller (slave only) - uses I2C or Serial communication interfaces - uses a dual H-bridge to drive 2 DC motors - uses quadrature encoders to get feedback from the motors, single interrupt mode, dual count (141x2=282CPR) - uses a trapezoidal profile to accelerate and decelerate - will use a PID control algorithm to keep a straight line - master has to keep track of traveled distance */ /* Distance calculations, specific to my robot: wheel diameter = 81.6mm -> wheel circumference = 81.6 x 3.1416 = 256.35456 mm distance between wheels = 160 mm -> turn circumference = 160 x 3.1416 = 502.656 mm mm / turn degree = 502.656 / 360 = 1.396 motor RPM = 80 -> RPS = 80/60 = 1.3333 encoder clics / wheel rotation = 282 mm / click = 256.3545 / 282 = 0.9090 clicks / mm = 282 / 256.35456 = 1.1 clicks / turn deg = 1.396 / 1.1 = 1.269 */ /* Driving and Turning with trapezoidal control profile For a given distance, we ramp up to set speed, drive or turn at a constant speed, then we ramp down to a stop; if the distance is too short, we ramp up to the middle of the distance then we ramp down to a stop. */ // R-Dev-Ino board pinout: // // Ext RDI Funct Arduino ATmega168 Arduino Funct RDI External // +-----\/----+ // Reset 1| PC6 PC5 |28 D19 A5 SCL // Rx D0 2| PD0 PC4 |27 D18 A4 SDA // Tx D1 3| PD1 PC3 |26 D17 A3 // E1A Int0 D2 4| PD2 PC2 |25 D16 A2 // E2A Int1 D3 5| PD3 PC1 |24 D15 A1 // D4 6| PD4 PC0 |23 D14 A0 // 7| VCC GND |22 // 8| GND AREF |21 // Xtal 9| PB6 AVCC |20 // Xtal 10| PB7 PB5 |19 D13 SCK LED // E2B OC0B D5 11| PD5 PB4 |18 D12 MISO M1B // E1B OC0A D6 12| PD6 PB3 |17 D11 OC2A MOSI // D7 13| PD7 PB2 |16 D10 OC1B M1A // M2B D8 14| PB0 PB1 |15 D 9 OC1A M2A // +-----------+ // #include //Inputs/outputs #define Encoder_1_ChA 2 // digital pin 2 // Right Encoder #define Encoder_1_ChB 6 // digital pin 6 #define Encoder_2_ChA 3 // digital pin 3 // Left Encoder #define Encoder_2_ChB 14 // digital pin 14 #define Motor_1_PWM 9 // digital pin 9 // Right Motor #define Motor_1_Dir 12 // digital pin 12 #define Motor_2_PWM 10 // digital pin 10 // Left Motor #define Motor_2_Dir 8 // digital pin 8 //Variables byte CpCm = 11; // clicks per centimeter byte CmMf = 1; // cm multiplying factor byte CpDg = 127; // clicks per degree (turn) byte DgMf = 100; // degree multiplying factor byte Acc = 10; byte direction = 1; byte PingDistance = 0; byte USValue[181]; byte x=0; byte y=0; byte a=0; volatile int Encoder_Count[2]={0,0}; #define E1 0 #define E2 1 byte I2C[5]={0,0,0,0,0}; #define Address 0 #define Command 1 #define Value1 2 #define Value2 3 // I2C addresses #define Brain 20>>1 #define Mapper 30>>1 #define Compass 42>>1 #define Speech 50>>1 #define MotorController 60>>1 #define ServoController 70>>1 #define LedBar 80>>1 #define Thermo 208>>1 // I2C commands #define Forward 1 //MotorController #define Reverse 2 //MotorController #define Left 3 //MotorController #define Right 4 //MotorController #define StopAll 5 //MotorController #define GoFwd 6 //MotorController #define GoBwd 7 //MotorController #define Pickup 8 //ServoController #define Release 9 //ServoController #define Wave 10 //ServoController #define Scan 11 //Mapper #define Fire 12 //Brain #define Fetch 13 //Brain #define Dance 14 //Brain #define ResetCounters() Encoder_Count[E1] = 0; Encoder_Count[E2] = 0 //----------------------------------------------------------------------------- void setup() { pinMode(Encoder_1_ChA, INPUT); //digitalWrite(Encoder_1_ChA, HIGH); // turn on pullup resistor pinMode(Encoder_1_ChB, INPUT); //digitalWrite(Encoder_1_ChB, HIGH); // turn on pullup resistor pinMode(Encoder_2_ChA, INPUT); //digitalWrite(Encoder_2_ChA, HIGH); // turn on pullup resistor pinMode(Encoder_2_ChB, INPUT); //digitalWrite(Encoder_2_ChB, HIGH); // turn on pullup resistor // set motor pins as output and LOW so the motors are breaked pinMode(Motor_1_PWM, OUTPUT); pinMode(Motor_1_Dir, OUTPUT); pinMode(Motor_2_PWM, OUTPUT); pinMode(Motor_2_Dir, OUTPUT); Braked(); //Serial.begin (9600); //Serial.println("start"); Wire.begin(MotorController); // join i2c bus with address #60 Wire.onReceive(I2CreceiveEvent); // register event attachInterrupt(0, doEncoder_1, CHANGE); // encoder pin on interrupt 0 attachInterrupt(1, doEncoder_2, CHANGE); // encoder pin on interrupt 1 } //----------------------------------------------------------------------------- void loop(){ Process_commands(); } //----------------------------------------------------------------------------- void Process_commands() { switch (I2C[Command]) { case 0: break; case Forward: direction = 1; //Serial.println("Moving Forward"); Drive(I2C[Value1], I2C[Value2]); // distance, speed break; case Reverse: direction = 0; //Serial.println("Moving Backward"); Drive(I2C[Value1], I2C[Value2]); // distance, speed break; case Left: direction = 2; //Serial.println("Turning Left"); Turn(I2C[Value1], I2C[Value2]); // angle, speed break; case Right: direction = 3; //Serial.println("Turning Right"); Turn(I2C[Value1], I2C[Value2]); // angle, speed break; case StopAll: //Serial.println("Braked!"); Braked(); break; case GoFwd: // Go Forward //Serial.println("Driving Forward"); digitalWrite(Motor_1_Dir, LOW); // forward analogWrite(Motor_1_PWM, I2C[Value2]); // Right speed digitalWrite(Motor_2_Dir, LOW); // forward analogWrite(Motor_2_PWM, I2C[Value1]); // Left speed break; case GoBwd: // Go Backward //Serial.println("Driving Backward"); digitalWrite(Motor_1_Dir, HIGH); // forward analogWrite(Motor_1_PWM, 255-I2C[Value2]); // Right speed digitalWrite(Motor_2_Dir, HIGH); // forward analogWrite(Motor_2_PWM, 255-I2C[Value1]); // Left speed break; } } //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ void Drive(int Distance, int Speed){ if (Distance > 0){ int Clicks = 0; int ConstSpeedDistance = 0; boolean Stopped=0; Distance = Distance * CpCm / CmMf; // convert distance from cm to clicks //Serial.print ("Distance: "); // debug - remember to comment out //Serial.println (Distance, DEC); // debug - remember to comment out ResetCounters(); for (int x=0; x<=Speed; x=x+Acc) { if (direction == 1){ // forward digitalWrite(Motor_1_Dir, LOW); // forward digitalWrite(Motor_2_Dir, LOW); // forward analogWrite(Motor_1_PWM, x); // direct PWM, low values mean low speed analogWrite(Motor_2_PWM, x); // }//end if if (direction == 0){ // reverse digitalWrite(Motor_1_Dir, HIGH); // reverse digitalWrite(Motor_2_Dir, HIGH); // reverse analogWrite(Motor_1_PWM, 255-x); // inverse PWM, low values mean high speed analogWrite(Motor_2_PWM, 255-x); // }//end if Clicks = (Encoder_Count[E1] + Encoder_Count[E2])/2; // read the encoders delay(8); //Serial.print ("Clicks: "); // debug - remember to comment out //Serial.println (Clicks, DEC); // debug - remember to comment out if (Clicks >= Distance/2){ // we don't have time to ramp up to max speed Stop(x); // so we start the deceleration x = Speed + Acc; // set x so we get out of the for loop //Clicks = (Encoder_Count[E1] + Encoder_Count[E2])/2; // read the encoders //delay(1); //Serial.print ("Clicks4: "); // debug - remember to comment out //Serial.println (Clicks, DEC); // debug - remember to comment out Stopped=1; }//end if if (!Stopped){ if (x > Speed-Acc) { // if we're close to the set speed ConstSpeedDistance = Distance - Clicks; // set the distance for constant speed //Serial.print ("CSD: "); // debug - remember to comment out //Serial.println (ConstSpeedDistance, DEC); // debug - remember to comment out }//end if }//end if }//end for if (!Stopped){ while (ConstSpeedDistance > Clicks) { // keep driving at set speed Clicks = (Encoder_Count[E1] + Encoder_Count[E2])/2; // read the encoders } Stop(Speed); // so we start the deceleration }//end if //Clicks = (Encoder_Count[E1] + Encoder_Count[E2])/2; // read the encoders //Serial.print ("Clicks3: "); // debug - remember to comment out //Serial.println (Clicks, DEC); // debug - remember to comment out }else{ if (direction == 1){ // forward digitalWrite(Motor_1_Dir, LOW); // forward digitalWrite(Motor_2_Dir, LOW); // forward analogWrite(Motor_1_PWM, Speed); // direct PWM, low values mean low speed analogWrite(Motor_2_PWM, Speed); // }//end if if (direction == 0){ // reverse digitalWrite(Motor_1_Dir, HIGH); // reverse digitalWrite(Motor_2_Dir, HIGH); // reverse analogWrite(Motor_1_PWM, 255-Speed); // inverse PWM, low values mean high speed analogWrite(Motor_2_PWM, 255-Speed); // }//end if } }//end void void Turn(int Angle, int Speed) { if (Angle>0){ int Clicks = 0; int ConstSpeedTurn = 0; boolean Stopped=0; Angle = Angle * CpDg / DgMf; // convert Angle from degrees to clicks //Serial.print ("Angle: "); // debug - remember to comment out //Serial.println (Angle, DEC); // debug - remember to comment out ResetCounters(); for (int x=0; x<=Speed; x=x+Acc) { if (direction == 2){ // right digitalWrite(Motor_1_Dir, HIGH); // reverse digitalWrite(Motor_2_Dir, LOW); // forward analogWrite(Motor_1_PWM, 255-x); // inverse PWM, low values mean high speed analogWrite(Motor_2_PWM, x); // direct PWM, low values mean low speed } if (direction == 3){ // left digitalWrite(Motor_1_Dir, LOW); // forward digitalWrite(Motor_2_Dir, HIGH); // reverse analogWrite(Motor_1_PWM, x); // direct PWM, low values mean low speed analogWrite(Motor_2_PWM, 255-x); // inverse PWM, low values mean high speed } Clicks = (Encoder_Count[E1] + Encoder_Count[E2])/2; // read the encoders delay(8); //Serial.print ("Clicks: "); // debug - remember to comment out //Serial.println (Clicks, DEC); // debug - remember to comment out if (Clicks >= Angle/2){ // we don't have time to ramp up to max speed Stop(x); // so we start the deceleration x = Speed + Acc; // set L so we get out of the for loop //Clicks = (Encoder_Count[E1] + Encoder_Count[E2])/2; // read the encoders //Serial.print ("Clicks4: "); // debug - remember to comment out //Serial.println (Clicks, DEC); // debug - remember to comment out Stopped=1; } if (!Stopped){ if (x > Speed-Acc) { // if we're close to the set speed ConstSpeedTurn = Angle - Clicks; // set the angle for constant speed //Serial.print ("CST: "); // debug - remember to comment out //Serial.println (ConstSpeedTurn, DEC); // debug - remember to comment out } } }//end for if (!Stopped){ while (ConstSpeedTurn > Clicks) { Clicks = (Encoder_Count[E1] + Encoder_Count[E2])/2; // read the encoders } Stop(Speed); } //Clicks = (Encoder_Count[E1] + Encoder_Count[E2])/2; // read the encoders //Serial.print ("Clicks3: "); // debug - remember to comment out //Serial.println (Clicks, DEC); // debug - remember to comment out }else{ if (direction == 2){ // right digitalWrite(Motor_1_Dir, HIGH); // reverse digitalWrite(Motor_2_Dir, LOW); // forward analogWrite(Motor_1_PWM, 255-Speed); // inverse PWM, low values mean high speed analogWrite(Motor_2_PWM, Speed); // direct PWM, low values mean low speed } if (direction == 3){ // left digitalWrite(Motor_1_Dir, LOW); // forward digitalWrite(Motor_2_Dir, HIGH); // reverse analogWrite(Motor_1_PWM, Speed); // direct PWM, low values mean low speed analogWrite(Motor_2_PWM, 255-Speed); // inverse PWM, low values mean high speed } } } void Stop(byte Speed){ // decelerate from set speed to stop switch (direction) { case 0: // direction = reverse for (int s=0; s <= Speed; s=s+Acc) { digitalWrite(Motor_1_Dir, HIGH); // reverse digitalWrite(Motor_2_Dir, HIGH); // reverse analogWrite(Motor_1_PWM, 255-Speed+s); // inverse PWM, low values mean high speed analogWrite(Motor_2_PWM, 255-Speed+s); // inverse PWM, low values mean high speed delay(3); } break; case 1: // direction = forward for (int s=0; s<=Speed; s=s+Acc) { digitalWrite(Motor_1_Dir, LOW); // forward digitalWrite(Motor_2_Dir, LOW); // forward analogWrite(Motor_1_PWM, Speed-s); analogWrite(Motor_2_PWM, Speed-s); delay(3); } break; case 2: // direction = left for (int s=0; s<=Speed; s=s+Acc) { digitalWrite(Motor_1_Dir, HIGH); // reverse digitalWrite(Motor_2_Dir, LOW); // forward analogWrite(Motor_1_PWM, 255-Speed+s); analogWrite(Motor_2_PWM, Speed-s); delay(3); } break; case 3: // direction = right for (int s=0; s<=Speed; s=s+Acc) { digitalWrite(Motor_1_Dir, LOW); // forward digitalWrite(Motor_2_Dir, HIGH); // reverse analogWrite(Motor_1_PWM, Speed-s); analogWrite(Motor_2_PWM, 255-Speed+s); // delay(3); } break; } } void Braked() { digitalWrite(Motor_1_PWM, LOW); digitalWrite(Motor_1_Dir, LOW); digitalWrite(Motor_2_PWM, LOW); digitalWrite(Motor_2_Dir, LOW); } //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ void I2CreceiveEvent(int i) { //Serial.println("I2C communication received:"); while(0 < Wire.available()) // loop through all { I2C[i] = Wire.receive(); // receive bytes //Serial.print(i, DEC); //Serial.print(" "); //Serial.println(I2C[i], DEC); i = i-1; } } //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ void doEncoder_1(){ Encoder_Count[E1]++; // CCW //Serial.print ("E1 count: "); // debug - remember to comment out //Serial.println (Encoder_Count[E1], DEC); // debug - remember to comment out } void doEncoder_2(){ Encoder_Count[E2]++; //Serial.print ("E2 count: "); // debug - remember to comment out //Serial.println (Encoder_Count[E2], DEC); // debug - remember to comment out }