Code Arduino to ROS

edit

#include <afmotor.h> #include <servo.h> #include <newping.h>

#define TRIG_PIN A4 // Pin A4 on the Motor Drive Shield soldered to the ultrasonic sensor #define ECHO_PIN A5 // Pin A5 on the Motor Drive Shield soldered to the ultrasonic sensor #define MAX_DISTANCE 200 // sets maximum useable sensor measuring distance to 200cm #define MAX_SPEED 180 // sets speed of DC traction motors to 180/256 or about 70% of full speed - to get power drain down. #define MAX_SPEED_OFFSET 10 // this sets offset to allow for differences between the two DC traction motors #define COLL_DIST 10 // sets distance at which robot stops and reverses to 10cm #define TURN_DIST COLL_DIST+10 // sets distance at which robot veers away from object (not reverse) to 20cm (10+10) NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure distance.

AF_DCMotor motor1(1, MOTOR12_1KHZ); // create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency AF_DCMotor motor2(4, MOTOR12_1KHZ); // create motor #2, using M2 output, set to 1kHz PWM frequency Servo myservo; // create servo object to control a servo

int pos = 0; // this sets up variables for use in the sketch (code) int maxDist = 0; int maxAngle = 0; int maxRight = 0; int maxLeft = 0; int maxFront = 0; int course = 0; int curDist = 0; String motorSet = ""; int speedSet = 0;

//-------------------------------------------- SETUP LOOP ---------------------------------------------------------------------------- void setup() { myservo.attach(9); // attaches the servo on pin 9 (SERVO_2 on the Motor Drive Shield to the servo object myservo.write(90); // tells the servo to position at 90-degrees ie. facing forward. delay(2000); // delay for two seconds checkPath(); // run the CheckPath routine to find the best path to begin travel motorSet = "FORWARD"; // set the director indicator variable to FORWARD myservo.write(90); // make sure servo is still facing forward moveForward(); // run function to make robot move forward } //------------------------------------------------------------------------------------------------------------------------------------

//---------------------------------------------MAIN LOOP ------------------------------------------------------------------------------ void loop() { checkForward(); // check that if the robot is supposed to be moving forward, that the drive motors are set to move forward - this is needed to overcome some issues with only using 4 AA NiMH batteries checkPath(); // set ultrasonic sensor to scan for any possible obstacles }