/* Robot Ver 1.5 Dan Kohn 05/21/2011 */ //debug #define debug LOW //set HIGH for debug output //Define Pins #define LED 13 //if LED on solid - no remote control signal #define LED1 31 #define LED2 33 #define LED4 35 #define vex 10 #define sonar_init 52 #define sonar_echo 8 #define compass_in 9 #define m1_I1 7 #define m1_I2 6 #define m1_E 5 #define m2_I3 4 #define m2_I4 3 #define m2_E 2 #define m1_encoder 18 #define m2_encoder 19 #define m1_int 5 #define m2_int 4 //temp variables int temp; long temp2; //motor speed variables int m1_speed; int m2_speed; int speed; //motor encoder volatile unsigned long m1_count; volatile unsigned long m2_count; //vex RC variables long duration; long center=1040; //long span=350; long span=100; long q[7]; int mode; boolean button[4]; long ch5_last; long ch6_last; //sonar varialbes unsigned long starttime; unsigned long endtime; unsigned long ttime; float distance; //compass variable unsigned long compass; float bearing; //heading float bearing_target; float bearing_db; float bearing_h; float bearing_l; //mode 3 int direction; //autocalibration mode 4 int step; float point[100]; void setup() { // initialize the digital pin as an output. pinMode(LED, OUTPUT); pinMode(LED1, OUTPUT); pinMode(LED2, OUTPUT); pinMode(LED4, OUTPUT); //vex remote control reciever pinMode(vex, INPUT); mode=0; //H-bridge drive motor control pinMode(m2_E, OUTPUT); pinMode(m2_I4, OUTPUT); pinMode(m2_I3, OUTPUT); pinMode(m1_E, OUTPUT); pinMode(m1_I2, OUTPUT); pinMode(m1_I1, OUTPUT); //sonar pins pinMode (sonar_echo, INPUT); pinMode (sonar_init, OUTPUT); //compass pin pinMode (compass_in, INPUT); bearing_db=5; //encoder m1_count=0; m2_count=0; attachInterrupt(m1_int,encode_m1,RISING); attachInterrupt(m2_int,encode_m2,RISING); direction=0; step=0; Serial.begin(9600); } void loop() { digitalWrite(LED,LOW); // read vex remote control reciever and store values in q[chn#] noInterrupts(); do // wait for long sync pulse { duration = pulseIn(vex,LOW); if (duration ==0) //if no remote stop everything! { digitalWrite(LED,HIGH); digitalWrite(m2_E,LOW); digitalWrite(m1_E,LOW); } } while(duration < 7000); for (int x=1; x<=6; x++) // read 6 valid readings and store in q[1]..q[6] { duration=pulseIn(vex,LOW); q[x]=duration; } // handle chn5 and chn6 buttons (set variable button[] to true or false // and creates a mode number based on the binary value. // 2^3 is used as a clear bit for the other 3 bits. temp2=q[5]-ch5_last; if ((q[5]>1500)&&(abs(temp2)>50)) { button[2]=!button[2]; } else if ((q[5]<500)&&(abs(q[5]-ch5_last)>50)) { // button[3]=!button[3]; mode=0; button[0]=LOW; button[1]=LOW; button[2]=LOW; button[3]=LOW; } temp2=q[6]-ch6_last; if ((q[6]>1500)&&(abs(temp2)>50)) { button[1]=!button[1]; } else if ((q[6]<600)&&(abs(temp2)>50)) { button[0]=!button[0]; } mode=button[0]*1+button[1]*2+button[2]*4; digitalWrite(LED1,button[0]); digitalWrite(LED2,button[1]); digitalWrite(LED4,button[2]); ch5_last=q[5]; ch6_last=q[6]; interrupts(); //display reading from vex remote control if (debug) { for (int x=1; x<=6; x++) { Serial.print(x); Serial.print(":\t"); Serial.print(q[x]); Serial.print("\t"); } Serial.print(mode); Serial.print("\t"); } //sonar read and display digitalWrite(sonar_init,HIGH); starttime=micros(); delayMicroseconds(10); digitalWrite(sonar_init,LOW); do { }while(digitalRead(sonar_echo)!=HIGH); do { }while(digitalRead(sonar_echo)!=LOW); endtime=micros(); ttime=endtime-starttime; delay((25000-ttime)/1000); //make the sonar routine take a constant time to run distance=ttime*pow(10,-6)*1087*12/2; if (debug) { Serial.print(ttime); Serial.print(" "); Serial.print(distance); Serial.print("\t"); } //read and display compass compass = pulseIn(compass_in,HIGH); delay((36990-compass)/1000); //make this routine run in a constant time bearing = (compass-1000)/100; if (debug) { Serial.print(compass); Serial.print("\t"); Serial.print(bearing); Serial.print("\t"); } // //stop if no com with vex controller // if (q[1]<200 || q[1]>3000 || q[2]<200 || q[2]>3000 || q[3]<200 || q[3]>3000 || q[4]<200 || q[4]>3000 || q[5]<200 || q[5]>3000 || q[6]<200 || q[6]>3000) //check for dead remote and stop // { // Serial.print ("no remote"); // digitalWrite(m2_E,LOW); // digitalWrite(m1_E,LOW); // } if (mode==0) //Remote control drive { if (debug) { Serial.print("->"); Serial.print(speed); Serial.print("\t"); } if ((q[2]>center+span)&&(q[1]>center-span)&&(q[1]255) speed=255; m1_speed=speed; m2_speed=speed; reverse(); } else if ((q[2]center-span)&&(q[1]255) speed=255; m1_speed=speed; m2_speed=speed; forward(); } else if (q[1]255) speed=255; m1_speed=speed; m2_speed=speed; left(); } else if(q[1]>center+span) { speed=abs(1040-q[1])*0.2561+150; if (speed>255) speed=255; m1_speed=speed; m2_speed=speed; right(); } else { mstop(); } } else if (mode==1) //sonar collision avoidance { m1_speed=255; m2_speed=255; if (distance>20) { forward(); } else { temp=random(0,99); if (temp <50) { right(); delay(200); mstop(); delay(200); } else { left(); delay(300); mstop(); delay(200); } } } else if (mode==2) //find bearing using campass { if (q[3]<660) bearing_target=0; else if (q[4]>1420) bearing_target=90; else if (q[3]>1420) bearing_target=180; else if (q[4]<660) bearing_target=270; else bearing_target=bearing; bearing_l = bearing_target-bearing_db/2; bearing_h = bearing_target+bearing_db/2; if (((bearing_target!=0)&&((bearingbearing_h)))||((bearing_target==0)&&((bearingbearing_h)))) { m1_speed=200; m2_speed=200; right(); delay(75); mstop(); } else mstop(); } else if (mode==3) //preprogrammed cycle { m1_speed=200; m2_speed=200; if ((direction==0)&&(m1_count<4000)) { forward(); } else if ((direction==0)&&(m1_count>4000)) { m1_count=0; direction=1; } else if ((direction==1)&&(m1_count<5200)) // aprox 90-100 deg turn { right(); } else if ((direction==1)&&(m1_count>5200)) { m1_count=0; direction=0; } } else if (mode==4) //autocalibration place robot down facing a wall about two feet away { if (step==0) { m1_speed=200; m2_speed=200; mstop(); step++; delay(1000); } else if (step==1) { point[0]=distance; //distance from wall to start m1_count=0; m2_count=0; step++; } else if (step==2) { forward(); delay(2000); mstop(); delay(1000); step++; } else if (step==3) { point[1]=distance; //distance from wall after move point[2]=m1_count; //number of counts m1 after move point[3]=m2_count; //number of counts m2 after move m1_count=0; m2_count=0; step++; } else if (step==4) { reverse(); delay(2000); mstop(); delay(1000); step++; } else if (step==5) { point[4]=distance; //after move back to start - distance from wall point[5]=m1_count; //number of counts m1 after move point[6]=m2_count; //number of counts m2 after move step++; } else if (step==6) { point[7]=bearing; //bearing before turn m1_count=0; m2_count=0; step++; } else if (step==7) { right(); delay(2000); mstop(); delay(1000); step++; } else if (step==8) { point[8]=bearing; //bearing after turn point[9]=m1_count; //number of counts m1 after turn point[10]=m2_count; //number of counts m2 after turn m1_count=0; m2_count=0; step++; } else if (step==9) { left(); delay(2000); mstop(); delay(1000); step++; } else if (step==10) { point[11]=bearing; //bearing after turn back to start point[12]=m1_count; //number of counts m1 after turn point[13]=m2_count; //number of counts m2 after turn step++; } else { Serial.print(step); Serial.print("\t"); for(temp=0;temp<=13;temp++) { Serial.print(point[temp]); Serial.print("\t"); } Serial.print("\n"); } } else { digitalWrite(m2_E,LOW); digitalWrite(m1_E,LOW); digitalWrite(LED,HIGH); delay(500); digitalWrite(LED,LOW); delay(500); step=0; } if (debug) { Serial.print("\t**\t"); Serial.print(m1_count); Serial.print("\t"); Serial.print(m2_count); Serial.print("\n"); } } void forward() { if (debug) Serial.print ("forward"); digitalWrite(m2_I4,LOW); digitalWrite(m2_I3,HIGH); digitalWrite(m1_I2,LOW); digitalWrite(m1_I1,HIGH); analogWrite(m2_E,m2_speed); analogWrite(m1_E,m1_speed); } void reverse() { if (debug) Serial.print ("reverse"); digitalWrite(m2_I4,HIGH); digitalWrite(m2_I3,LOW); digitalWrite(m1_I2,HIGH); digitalWrite(m1_I1,LOW); analogWrite(m2_E,m2_speed); analogWrite(m1_E,m1_speed); } void left() { if (debug) Serial.print ("left"); digitalWrite(m2_I4,HIGH); digitalWrite(m2_I3,LOW); digitalWrite(m1_I2,LOW); digitalWrite(m1_I1,HIGH); analogWrite(m2_E,m2_speed); analogWrite(m1_E,m1_speed); } void right() { if (debug) Serial.print ("right"); digitalWrite(m2_I4,LOW); digitalWrite(m2_I3,HIGH); digitalWrite(m1_I2,HIGH); digitalWrite(m1_I1,LOW); analogWrite(m2_E,m2_speed); analogWrite(m1_E,m1_speed); } void mstop() { if (debug) Serial.print ("stop"); digitalWrite(m2_E,LOW); digitalWrite(m1_E,LOW); } void encode_m1() { m1_count++; } void encode_m2() { m2_count++; }