User:Bzeher/enes100/RemoteCar/SonarCode

int inApin[2] = {7, 4}; // INA: Clockwise input int inBpin[2] = {8, 9}; // INB: Counter-clockwise input int pwmpin[2] = {5, 6}; // PWM input int cspin[2] = {2, 3}; // CS: Current sense ANALOG input int enpin[2] = {0, 1}; // EN: Status of switches output (Analog pin) int statpin = 13;      //motor pin NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); Servo servoMain; long Mdistance, Rdistance, Ldistance;
 * 1) include 
 * 2) include 
 * 3) define TRIGGER_PIN 12
 * 4) define ECHO_PIN    11
 * 5) define MAX_DISTANCE 45
 * 6) define BRAKEVCC 0
 * 7) define CW  1
 * 8) define CCW 2
 * 9) define BRAKEGND 3
 * 10) define CS_THRESHOLD 200

int distanceM = 0; int distance12 = 0; int distance13 = 0; int distance21 = 0; int distance22 = 0; void setup { servoMain.attach(13); Serial.begin(9600); }

void loop { distanceM = 0; distance12 = 0; distance13 = 0; distance21 = 0; distance22 = 0; motorGo(0, 0, 0);    //Keep straight motorGo(1, CW, 80);   //Forward servoMain.write(90); delay(120); distanceM = sonar.ping_cm; delay(100); if(distanceM > 0) {  motorGo(1, 0, 0);  //Stop servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(105); delay(500); distance12 = sonar.ping_cm; delay(100); servoMain.write(120); delay(500); distance13 = sonar.ping_cm; delay(100); servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(75); delay(500); distance21 = sonar.ping_cm; delay(100); servoMain.write(60); delay(500); distance22 = sonar.ping_cm; delay(100); if(distanceM > 0 ) {   motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(1200); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(1200); motorGo(0, CCW, 0); //stop turn } else if(distance12 > 0 || distance13 > 0) {   motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn }    else if(distance21 > 0 || distance22 > 0) {    motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn }   distanceM = 0; distance12 = 0; distance13 = 0; distance21 = 0; distance22 = 0; }

servoMain.write(105); delay(120); distance12 = sonar.ping_cm; delay(100); if(distance12 > 0) {  motorGo(1, 0, 0);  //Stop servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(105); delay(500); distance12 = sonar.ping_cm; delay(100); servoMain.write(120); delay(500); distance13 = sonar.ping_cm; delay(100); servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(75); delay(500); distance21 = sonar.ping_cm; delay(100); servoMain.write(60); delay(500); distance22 = sonar.ping_cm; delay(100); if(distanceM > 0 ) {   motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(1200); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(1200); motorGo(0, CCW, 0); //stop turn } else if(distance12 > 0 || distance13 > 0) {    motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn }    else if(distance21 > 0 || distance22 > 0) {    motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn }   distanceM = 0; distance12 = 0; distance13 = 0; distance21 = 0; distance22 = 0; }  servoMain.write(120); delay(120); distance13 = sonar.ping_cm; delay(100); if(distance13 > 0) {  motorGo(1, 0, 0);  //Stop servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(105); delay(500); distance12 = sonar.ping_cm; delay(100); servoMain.write(120); delay(500); distance13 = sonar.ping_cm; delay(100); servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(75); delay(500); distance21 = sonar.ping_cm; delay(100); servoMain.write(60); delay(500); distance22 = sonar.ping_cm; delay(100); if(distanceM > 0 ) {   motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(1200); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(1200); motorGo(0, CCW, 0); //stop turn } else if(distance12 > 0 || distance13 > 0) {    motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn }    else if(distance21 > 0 || distance22 > 0) {    motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn }   distanceM = 0; distance12 = 0; distance13 = 0; distance21 = 0; distance22 = 0; }  servoMain.write(90); delay(120); distanceM = sonar.ping_cm; delay(100); if(distanceM > 0) {  motorGo(1, 0, 0);  //Stop servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(105); delay(500); distance12 = sonar.ping_cm; delay(100); servoMain.write(120); delay(500); distance13 = sonar.ping_cm; delay(100); servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(75); delay(500); distance21 = sonar.ping_cm; delay(100); servoMain.write(60); delay(500); distance22 = sonar.ping_cm; delay(100); if(distanceM > 0 ) {   motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(1200); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(1200); motorGo(0, CCW, 0); //stop turn } else if(distance12 > 0 || distance13 > 0) {    motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn }    else if(distance21 > 0 || distance22 > 0) {    motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn }   distanceM = 0; distance12 = 0; distance13 = 0; distance21 = 0; distance22 = 0; }  servoMain.write(75); delay(120); distance21 = sonar.ping_cm; delay(100); if(distance21 > 0) {  motorGo(1, 0, 0);  //Stop servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(105); delay(500); distance12 = sonar.ping_cm; delay(100); servoMain.write(120); delay(500); distance13 = sonar.ping_cm; delay(100); servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(75); delay(500); distance21 = sonar.ping_cm; delay(100); servoMain.write(60); delay(500); distance22 = sonar.ping_cm; delay(100); if(distanceM > 0 ) {   motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(1200); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(1200); motorGo(0, CCW, 0); //stop turn } else if(distance12 > 0 || distance13 > 0) {    motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn }    else if(distance21 > 0 || distance22 > 0) {    motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn }  distanceM = 0; distance12 = 0; distance13 = 0; distance21 = 0; distance22 = 0; }  servoMain.write(60); delay(120); distance22 = sonar.ping_cm; delay(100); if(distance22 > 0) {  motorGo(1, 0, 0);  //Stop servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(105); delay(500); distance12 = sonar.ping_cm; delay(100); servoMain.write(120); delay(500); distance13 = sonar.ping_cm; delay(100); servoMain.write(90); delay(500); distanceM = sonar.ping_cm; delay(100); servoMain.write(75); delay(500); distance21 = sonar.ping_cm; delay(100); servoMain.write(60); delay(500); distance22 = sonar.ping_cm; delay(100); if(distanceM > 0 ) {   motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(1200); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(1200); motorGo(0, CCW, 0); //stop turn } else if(distance12 > 0 || distance13 > 0) {    motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn }    else if(distance21 > 0 || distance22 > 0) {    motorGo(1, 0, 0);  //Stop                                          //Motor cotrol delay(300); motorGo(0, CCW, 200); //Turn Left motorGo(1, CCW, 100); //Revearse delay(800); motorGo(1, 0, 0); // Stop delay(300); motorGo(0, CW, 200); //Turn Right motorGo(1, CW, 100); //Forward delay(800); motorGo(0, CCW, 0); //stop turn } }

//Threshold reached if ((analogRead(cspin[0]) < CS_THRESHOLD) && (analogRead(cspin[1]) < CS_THRESHOLD)) digitalWrite(statpin, HIGH); }

void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm) { if (motor <= 1) { if (direct <=4) {   // Set inA[motor] if (direct <=1) digitalWrite(inApin[motor], HIGH); else digitalWrite(inApin[motor], LOW); // Set inB[motor] if ((direct==0)||(direct==2)) digitalWrite(inBpin[motor], HIGH); else digitalWrite(inBpin[motor], LOW); analogWrite(pwmpin[motor], pwm); } } }

void motorOff(int motor) { // Initialize braked for (int i=0; i<2; i++) { digitalWrite(inApin[i], LOW); digitalWrite(inBpin[i], LOW); } analogWrite(pwmpin[motor], 0); }