/* Robot Ver 1.33 Dan Kohn 05/19/2011 */ //debug #define debug LOW //set HIGH for debug output //Define Pins #define LED 13 //if LED on solid - no remote control signal #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=50; boolean chn5; boolean chn6; long q[7]; //sonar varialbes unsigned long starttime; unsigned long endtime; unsigned long ttime; float distance; //compass variable unsigned long compass; float bearing; //mode 3 int direction; void setup() { // initialize the digital pin as an output. pinMode(LED, OUTPUT); //vex remote control reciever pinMode(vex, INPUT); //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); //encoder m1_count=0; m2_count=0; attachInterrupt(m1_int,encode_m1,RISING); attachInterrupt(m2_int,encode_m2,RISING); direction=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 variables chn5 and chn6 (boolean) to true or false if (q[5]>1500) { chn5=LOW; mstop(); } else if (q[5]<500) { chn5=HIGH; mstop(); } if (q[6]>1500) { chn6=LOW; mstop(); } else if (q[6]<600) { chn6=HIGH; mstop(); } 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("*"); Serial.print(chn5,BIN); Serial.print("\t"); Serial.print(chn6,BIN); 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; 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); 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 (chn5==LOW && chn6==LOW) //Remote control drive { if (debug) { Serial.print("->"); Serial.print(speed); Serial.print("\t"); } if ((q[2]>center+span)&&(q[1]>center-span)&&(q[1]