Dagu rover 5 - arduino mega 2560 - Xbee (9 / 11 paso)

Paso 9: Código Final

Aquí está el código Final para el arduino mega 2560. He creado sub comandos para hacer más fácil.

 int count; int check1; //see if subcomand has run int check2; //see if subcomand has run int check3; //see if subcomand has run int check4; //see if subcomand has run int check5; //see if subcomand has run 

 void setup() { Lcount=0; Rcount=0; 

 // initialize the digital pin as an output. pinMode(dirch1,OUTPUT); pinMode(dirch2,OUTPUT); pinMode(dirch3,OUTPUT); pinMode(dirch4,OUTPUT); 

 pinMode(speedch1,OUTPUT); pinMode(speedch2,OUTPUT); pinMode(speedch3,OUTPUT); pinMode(speedch4,OUTPUT); 

 pinMode(encoderA, INPUT); pinMode(encoderB, INPUT); attachInterrupt(0, LhandleEncoder, CHANGE); pinMode(encoderC, INPUT); pinMode(encoderD, INPUT); attachInterrupt(5, RhandleEncoder, CHANGE); 

 Serial2.begin(9600); Serial.begin(9600); 

 analogWrite(speedch1,0);// stop all analogWrite(speedch2,0);// stop all analogWrite(speedch3,0);// stop all analogWrite(speedch4,0);// stop all 

 count = 0; (SPEED = 200); 

 } 

 // the loop routine runs over and over again forever: void loop() { 

 if (Serial2.available() > 0) { // read the incoming byte: incomingByte = Serial2.read(); } if(incomingByte == 10 && check1 == 0) forward(); else if(incomingByte == 11 && check2 == 0) rotate_right(); else if(incomingByte == 12 && check3 == 0) backward(); else if(incomingByte == 13 && check4 == 0) rotate_left(); else if(incomingByte == 14 && check5 == 0) pause(); 

 // if(incomingByte == 16){ // if (Lcount != Lcountsaved && Rcount != Rcountsaved) Serial2.print(Lcount,DEC);//send encoder data Serial2.print(","); Serial2.print(Rcount,DEC); Serial2.print(","); Serial2.println(); incomingByte = 1; // Lcountsaved = Lcount; //Rcountsaved = Rcount; // } } void LhandleEncoder() { if(digitalRead(encoderA) == digitalRead(encoderB)) { Lcount++; } else { Lcount--; } } 

 void RhandleEncoder() { if(digitalRead(encoderC) == digitalRead(encoderD)) { Rcount++; } else { Rcount--; } } 

 void forward() { check2 = 0; check3 = 0; check4 = 0; check5 = 0; analogWrite(speedch1,0); analogWrite(speedch2,0); analogWrite(speedch3,0); analogWrite(speedch4,0); delay(100); digitalWrite(dirch1,LOW); digitalWrite(dirch2,HIGH); digitalWrite(dirch3,LOW); digitalWrite(dirch4,HIGH); analogWrite(speedch1,SPEED); analogWrite(speedch2,SPEED); analogWrite(speedch3,SPEED); analogWrite(speedch4,SPEED); check1 = 1; 

una y otra vez funciona para siempre la rutina bucle: } 

Si (Serial2.available() > 0) {/ / lee el byte entrante: incomingByte = Serial2.read(); void backward() { check1 = 0; check2 = 0; check4 = 0; check5 = 0; analogWrite(speedch1,0); analogWrite(speedch2,0); analogWrite(speedch3,0); analogWrite(speedch4,0); delay(100); digitalWrite(dirch1,HIGH); digitalWrite(dirch2,LOW); digitalWrite(dirch3,HIGH); digitalWrite(dirch4,LOW); analogWrite(speedch1,SPEED); analogWrite(speedch2,SPEED); analogWrite(speedch3,SPEED); analogWrite(speedch4,SPEED); 

if(incomingByte == 16) {/ / si (Lcount! = Lcountsaved & & Rcount! = Rcountsaved) Serial2.print (Lcount, DEC); //send codificador datos Serial2.print(","); Serial2.Print(Rcount,DEC); Serial2.Print(","); Serial2.println(); incomingByte = 1; Lcountsaved = Lcount; Rcountsaved = Rcount; // check3 = 1; } void LhandleEncoder() {if(digitalRead(encoderA) == digitalRead(encoderB)) {Lcount ++; } Else {Lcount--; void rotate_right() { check1 = 0; check3 = 0; check4 = 0; check5 = 0; analogWrite(speedch1,0); analogWrite(speedch2,0); analogWrite(speedch3,0); analogWrite(speedch4,0); delay(100); //wait to stop digitalWrite(dirch1,LOW); digitalWrite(dirch2,HIGH); digitalWrite(dirch3,HIGH); digitalWrite(dirch4,LOW); analogWrite(speedch1,SPEED); analogWrite(speedch2,SPEED); analogWrite(speedch3,SPEED); analogWrite(speedch4,SPEED); check2 = 1; 

void RhandleEncoder() {if(digitalRead(encoderC) == digitalRead(encoderD)) {Rcount ++; } Else {Rcount--; void rotate_left() { check1 = 0; check2 = 0; check3 = 0; check5 = 0; analogWrite(speedch1,0); analogWrite(speedch2,0); analogWrite(speedch3,0); analogWrite(speedch4,0); delay(100); digitalWrite(dirch1,HIGH); digitalWrite(dirch2,LOW); digitalWrite(dirch3,LOW); digitalWrite(dirch4,HIGH); analogWrite(speedch1,SPEED); analogWrite(speedch2,SPEED); analogWrite(speedch3,SPEED); analogWrite(speedch4,SPEED); check4 = 1; 

 } 

 void pause() { check1 = 0; check2 = 0; check3 = 0; check4 = 0; analogWrite(speedch1,0); analogWrite(speedch2,0); analogWrite(speedch3,0); analogWrite(speedch4,0); check5 = 1; } 

 import processing.serial.*; Serial myPort; // Create object from Serial class int lf = 10; // Linefeed in ASCII int source1; int source2; String myString = null; 

 void setup(){ size(640, 360); println(Serial.list()); String portName = Serial.list()[5]; // pick the port connected myPort = new Serial(this, portName, 9600); myPort.clear(); 

 myString = myPort.readStringUntil(lf); myString = null; source1 = 0; source2 = 0; 

 } void draw(){ background(255,255,255); textSize(32); text(source1,20,60); text(source2,20,120); fill(0,0,0); while (myPort.available() > 0) { myString = myPort.readStringUntil(lf); if (myString != null) { println(myString); 

 String[] q = splitTokens(myString, ",");//split at "," println(q.length + " values found"); // Prints 2 values found" source1 = int(q[0]); source2 = int(q[1]); } } } void keyPressed() { if (key == CODED) { if (keyCode == UP) { myPort.write(10);//Forward } else if (keyCode == DOWN) { myPort.write(12);//Backward } else if(keyCode == LEFT){ myPort.write(13);//LEFT } else if(keyCode == RIGHT){ myPort.write(11);//Right }else if(keyCode == CONTROL){ myPort.write(14); //Pause } } } 

void pause() {check1 = 0; check2 = 0; check3 = 0; check4 = 0; analogWrite(speedch1,0); analogWrite(speedch2,0); analogWrite(speedch3,0); analogWrite(speedch4,0); check5 = 1;