Thursday, October 11, 2012

Arduino - Control ESC/Motor (Arduino Code Ex.2)

The following code below is posted as another example of how to control more than one motor/esc with an Arduino.

I am using the Servo library to control two motor/esc.  The first motor is called the left and the second motor is called the right motor.  I send the speed in the following format (without quotes):

"l76;r88;n"

In the example above, "l76;" is the speed that I want to send to the left motor.  In the example above, "r88; is the speed that I want to send to the right motor.  In the example above, "n" is how I tell the arduino that the message is complete and done.  

When using the Servo library, we can arm the esc/motors by incrementally stepping up to 60.  NOTE:  This may vary for your esc/motor,  test with caution.  For me, setting the value above 65 put the motors in motion.  Writing anything less than 65 to the motors will stop the motors.  For this to happen,  

The code below is posted as additional content that is free to do whatever you want.  (use, manipulate, copy, learn).  I'm publishing this as something that is not as clean because I believe people want/need more examples.  There is one reference I have for this project that helped me to understand the arming procedure. I can't find it at the moment, and will post it later. 





 #include "Servo.h"  
 #define MOTOR_PIN  9 // This is the left motor  
 #define MOTOR_PIN_R           10  
 #define MOTOR_MAX_SPEED     90 // NOT REALLY NEEDED  
 #define MOTOR_START_SPEED        60  
 /*  
  * This is the max/min speed values restriction  
  */  
 int motor_left_minimum = 68;  
 int motor_left_maximum = 118;  
 int motor_right_minimum = 68;  
 int motor_right_maximum = 118;  
 int motor_current_speed = 0;  
 Servo motor; // This is the left motor  
 Servo motorRight;  
 String left;  
 String right;  
 void setup()  
 {  
  // 115200 default for blusmirf gold  
  //Serial.begin(115200);  
     // 9600 default for the xbee pro series1  
     Serial.begin(9600);  
  // Motor  
  delay(1000);  
  motor.attach(MOTOR_PIN); // This is the left motor  
     motorRight.attach(MOTOR_PIN_R);  
  motorStartAt(MOTOR_START_SPEED); // Arming procedure  
  delay(1500);  
 }  
 void loop()  
 {  
  //Serial.println("in loop");  
  if(Serial.available()>0){  
  //String myString = Serial.readStringUntil('\n');  
  //Serial.println(myString);  
   char ch = Serial.read();  
    if(ch == 'l')  
    {  
     do{  
      if(ch > 47 && ch < 58)  
       {  
       //Serial.print("I have received: ");  
       //Serial.print(ch, DEC);  
       //Serial.print('\n');  
       left += ch;  
       }  
       ch = Serial.read();  
      }while(ch != ';');  
    }  
    else if(ch == 'r')  
     do{  
      if(ch > 47 && ch < 58)  
      {  
      //Serial.print("I have received: ");  
      //Serial.print(ch,DEC);  
      //Serial.print('\n');  
      right += ch;  
      }  
      ch = Serial.read();  
      }while(ch != ';');  
    else if(ch == 'n')  
    {  
    int leftValue = left.toInt();  
    int rightValue = right.toInt();  
    Serial.print("Left value is: ");  
    Serial.print(leftValue);  
    Serial.print(" : ");// + " and " + right);  
    Serial.print("Right value is: ");  
    Serial.print(rightValue);  
    Serial.print('\n');  
    Serial.println();  
    if(leftValue >= motor_left_minimum && leftValue <= motor_left_maximum && rightValue >= motor_right_minimum && rightValue <= motor_right_maximum)  
    {  
     if(leftValue == motor_left_minimum)  
     {  
     motor.write(60);  
     //Serial.println("Left motor set to 60");  
     }  
     else  
     {  
     motor.write(leftValue);  
     /*Serial.print("Left motor set to ");  
     Serial.print(leftValue);  
     Serial.println();  
   */  
     }  
     if(rightValue == motor_right_minimum)  
     {  
     motorRight.write(60);  
     //Serial.println("Right motor set to 60");  
     }  
     else  
     {  
     motorRight.write(rightValue);  
     /*Serial.print("Right motor set to ");  
     Serial.print(rightValue);  
     Serial.println();*/  
     }  
    }  
    left = "";  
    right = "";  
    leftValue = 0;  
    rightValue = 0;  
   }  
  }  
 }  
 // Wrapper function for Servo's ".write(*speed*)" function  
 void motorSetSpeed(int speed)  
 {  
   // Don't let the motor go above or below pre-determined max and min  
  if (speed > MOTOR_MAX_SPEED)  
  speed = MOTOR_MAX_SPEED;  
  else if (speed < MOTOR_START_SPEED)  
  speed = MOTOR_START_SPEED;  
  motor.write(speed);  
     motorRight.write(speed);  
  motor_current_speed = speed;  
  //Serial.print("current motor speed = ");  
  //Serial.println(motor_current_speed);  
 }  
 void motorStartAt(int start_speed)  
 {  
  int i;  
  for (i=0; i < start_speed; i+=5) {  
  motorSetSpeed(i);  
  //Serial.println(i);  
  delay(100);  
  }  
 }