Tuesday, January 31, 2012

Making sense of giving sense to the motor shield

 The youtube video by Stevan Dedovac is here:

http://www.youtube.com/watch?v=V-92YF87huo



Here is the code we hope to hack for using the Parallax Ping sensor we have with the Adafruit Motor Controller.



from https://github.com/bandless55/Arduino-Robot/blob/master/ping_robot.pde

by Stevan Dedovic authored August 13, 2011



/* please ignore any spelling errors.

Thanks. also, feel free to use this

for whatever you want.

(as long as i get to see thoes creations)

*/


#include <AFMotor.h>

AF_DCMotor motor(2, MOTOR12_1KHZ);

AF_DCMotor turn(1, MOTOR12_1KHZ); //Motor Shield Stuff



int pingPin = A5; //


int turnDelay = 2000; //how long a turn is. A larger number means a larger turn. and vice versa.


void setup(){ //runs the motor for a really short time and then waits 3 sec.

              //This just gives me time to put the motor down before it starts up.

  motor.setSpeed(175);

  turn.setSpeed(255);

  motor.run(FORWARD);

  delay(25);

  motor.run(RELEASE);

  delay(3000);

}


void loop(){

  int distance = 7; //distance, in inches, that triggers the turn sequence

 

 

  int x = 0;

  x = ping(); //get distance


 

  if(x > distance){

    motor.run(FORWARD);

  }

 

  else if(x < distance){

  

    motor.run(BACKWARD); //instantly stop

    delay(250);

    left();

    motor.run(RELEASE); //THIS RELEASE AND DELAY IS ESSENTIAL.

    delay(100);

  

   /* So about that comment above. I found out that after turning the robot in any direction,

the use of both the turning motor/servo and the movement motor used up all the power for the device.

So everytime the robot turns, I make sure to wait a little after cutting off power to both motors.

This ensures that the ping))) sensor recieves all the power from the batteries.

Then it reruns the loop and checks the distance, and either moves foward or turns.

*/

 

  }

}



unsigned long ping(){ //arduino ping example: http://www.arduino.cc/en/Tutorial/Ping

  long duration, inches, cm;

  pinMode(pingPin, OUTPUT);

  digitalWrite(pingPin, LOW);

  delayMicroseconds(2);

  digitalWrite(pingPin, HIGH);

  delayMicroseconds(5);

  digitalWrite(pingPin, LOW);

  pinMode(pingPin, INPUT);

  duration = pulseIn(pingPin, HIGH);

  inches = microsecondsToInches(duration);

  cm = microsecondsToCentimeters(duration);

  return inches;

}


void left(){ //turns the robot to the left

    turn.run(BACKWARD);

    delay(turnDelay - 100);

    turn.run(FORWARD);

    delay(100);

    motor.run(FORWARD);

    delay(turnDelay);

    turn.run(RELEASE);

}


void right(){ //turns the robot to the right

  turn.run(FORWARD);

    delay(turnDelay - 100);

    turn.run(BACKWARD);

    delay(100);

    motor.run(FORWARD);

    delay(turnDelay);

    turn.run(RELEASE);

}


long microsecondsToInches(long microseconds) //more ping stuff

{

  // According to Parallax's datasheet for the PING))), there are

  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per

  // second). This gives the distance travelled by the ping, outbound

  // and return, so we divide by 2 to get the distance of the obstacle.

  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf

  return microseconds / 74 / 2;

}


long microsecondsToCentimeters(long microseconds) //even more ping stuff

{

  // The speed of sound is 340 m/s or 29 microseconds per centimeter.

  // The ping travels out and back, so to find the distance of the

  // object we take half of the distance travelled.

  return microseconds / 29 / 2;

}




I've rewritten the code to include the other motor and eliminating the turn motor so we can run the Sea Sparrow:

/* please ignore any spelling errors.
Thanks. also, feel free to use this
for whatever you want.
(as long as i get to see thoes creations)
*/

#include <AFMotor.h>
AF_DCMotor rightmotor(3, MOTOR12_1KHZ);
AF_DCMotor leftmotor(1, MOTOR12_1KHZ); //Motor Shield Stuff


int pingPin = A5; //

int leftmotorDelay = 2000; //how long a turn is. A larger number means a larger turn. and vice versa.
int rightmotorDelay = 2000; //

void setup(){ //runs the motor for a really short time and then waits 3 sec.
              //This just gives me time to put the motor down before it starts up.
             
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Sea Sparrow Motor test!");           
  rightmotor.setSpeed(175);
  leftmotor.setSpeed(175);
  rightmotor.run(FORWARD);
  leftmotor.run(FORWARD);
  delay(25);
  rightmotor.run(RELEASE);
  leftmotor.run(RELEASE);
  delay(3000);

}
void loop(){
 

  int distance = 7; //distance, in inches, that triggers the turn sequence
  int x = 0;
  x = ping(); //get distance
if(x > distance){

    rightmotor.run(FORWARD);
    leftmotor.run(FORWARD);
 
  
  }

    else if(x < distance){
  
    rightmotor.run(BACKWARD); //instantly stop
    leftmotor.run(BACKWARD);
    delay(250);
  left();
    rightmotor.run(RELEASE); //THIS RELEASE AND DELAY IS ESSENTIAL.
    leftmotor.run(RELEASE); //THIS RELEASE AND DELAY IS ESSENTIAL.
    delay(100);
      }
 
}
     

  /* So about that comment above. I found out that after turning the robot in any direction,
//the use of both the turning motor/servo and the movement motor used up all the power for the device.
//So everytime the robot turns, I make sure to wait a little after cutting off power to both motors.
//This ensures that the ping))) sensor recieves all the power from the batteries.
//Then it reruns the loop and checks the distance, and either moves foward or turns.
*/



unsigned long ping(){ //arduino ping example: http://www.arduino.cc/en/Tutorial/Ping
  long duration, inches, cm;
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);
  return inches;
}

void left(){ //turns the robot to the left
    leftmotor.run(BACKWARD);
    delay(leftmotorDelay - 100);
   leftmotor.run(RELEASE);
    delay(100);
    rightmotor.run(FORWARD);
    delay(leftmotorDelay);
    rightmotor.run(RELEASE);
}

void right(){ //turns the robot to the right
  rightmotor.run(BACKWARD);
    delay(rightmotorDelay - 100);
    leftmotor.run(RELEASE);
    delay(100);
    rightmotor.run(FORWARD);
    delay(rightmotorDelay);
    rightmotor.run(RELEASE);
}

long microsecondsToInches(long microseconds) //more ping stuff
{
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second). This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds) //even more ping stuff
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}


HERE IS SOME TRIAL CODE TO MERGE THE TWO:


#include <AFMotor.h>
AF_DCMotor rightmotor(3, MOTOR12_1KHZ);
AF_DCMotor leftmotor(1, MOTOR12_1KHZ); //Motor Shield Stuff

int pingPin = A5; //

int leftmotorDelay = 2000; //how long a turn is. A larger number means a larger turn. and vice versa.
int rightmotorDelay = 2000; //


unsigned long ping(){ //arduino ping example: http://www.arduino.cc/en/Tutorial/Ping
  long duration, inches, cm;
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);
  return inches;
}

void left(){ //turns the robot to the left
    leftmotor.run(BACKWARD);
    delay(leftmotorDelay - 100);
   leftmotor.run(RELEASE);
    delay(100);
    rightmotor.run(FORWARD);
    delay(leftmotorDelay);
    rightmotor.run(RELEASE);
}

void right(){ //turns the robot to the right
  rightmotor.run(BACKWARD);
    delay(rightmotorDelay - 100);
    leftmotor.run(RELEASE);
    delay(100);
    rightmotor.run(FORWARD);
    delay(rightmotorDelay);
    rightmotor.run(RELEASE);
}

long microsecondsToInches(long microseconds) //more ping stuff
{
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second). This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds) //even more ping stuff
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

bool readSerial () {
  char in = Serial.read();
  switch(in) {
    case 'f':
      rightmotor.run(FORWARD);
  leftmotor.run(FORWARD);
  case 'b':
  rightmotor.run(BACKWARD);
  leftmotor.run(BACKWARD);
  case 's':
  rightmotor.run(RELEASE);
  leftmotor.run(RELEASE);
  default:
  return false;
  }
  return true;
}

bool delayWithSerial (unsigned int time) { ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// this is basically what you need to write
  unsigned int startTime = millis();
  while(millis() - startTime > time) {
    if(Serial.available()){
      // if the readSerial function returns true, then this gives the feed back that it needs to exit the loop and be overwritten
      if(readSerial()) return true;
    }
  }
  return false; // here we are saying that we ended the time normally so we should keep going with the loop
}

void setup () {
  Serial.begin(9600); // I personally like 115200 better, or maybe 38400
  Serial.println("Sea Sparrow Motor test!");           
  rightmotor.setSpeed(175);
  leftmotor.setSpeed(175);
  rightmotor.run(FORWARD);
  leftmotor.run(FORWARD);
  delayWithSerial(25);
  rightmotor.run(RELEASE);
  leftmotor.run(RELEASE);
  delayWithSerial(3000);
}



void loop () {
  if(Serial.available()) {
    readSerial();
  }
  if(ping() < 3) {
    rightmotor.run(BACKWARD); //instantly stop
    leftmotor.run(BACKWARD);
    if(delayWithSerial(250)) return; // here we get the return from from the read Serial based back and then break the loop funciton, this should return it to the top of the loop function
  left();
    rightmotor.run(RELEASE); //THIS RELEASE AND DELAY IS ESSENTIAL.
    leftmotor.run(RELEASE); //THIS RELEASE AND DELAY IS ESSENTIAL.
    if(delayWithSerial(100)) return; // see comment above
  }
}


Now that we are using the DFRobot 2A Motor Shield, we need to learn its coding too. Here is an example:

//Arduino PWM Speed Control:
int E1 = 6;  
int M1 = 7;
int E2 = 5;                        
int M2 = 4;                          

void setup()
{
    pinMode(M1, OUTPUT);  
    pinMode(M2, OUTPUT);
}

void loop()
{
//  int value;
// for(value = 0 ; value <= 255; value+=5)
//
//  {
    digitalWrite(M1,HIGH);  
    digitalWrite(M2, HIGH);      
//    analogWrite(E1, value);   //PWM Speed Control
//    analogWrite(E2, value);   //PWM Speed Control
    analogWrite(E1, 255);   //PWM Speed Control
    analogWrite(E2, 255);   //PWM Speed Control
    delay(30);
//  } 
}

No comments:

Post a Comment