Paso 2: código
Muy bien - así - cuando se trabaja con código es ideal para usar algo como Github seguimiento de revisiones. No hacerlo. Así, accidentalmente escribí sobre la versión más actual de trabajo del código. Tengo la última versión guardada, pero no estoy seguro cómo es. Probablemente es un poco buggy.
Novertheless, ya que ninguno de ustedes es probable que construirá esta cosa cualquier que forma y la cama está en pedazos y no funciona, yo solo voy a publicar lo que tengo.
También estoy compartiendo código de prueba para el control manual de los motores. Si usted está buscando en este proyecto para interfaces de un Arduino a motores muy grandes a través de un regulador del motor de Alltrax, este código será más útil de todos modos.
Última versión guardada de código de prueba de la cama de Robot:
/*This example code is in the public domain. */ //establish throttle pins int leftThrottle = 3; // LED connected to digital pin 9 int rightThrottle = 5; // LED connected to digital pin 9 //establish solenoid pins int leftOn = 7; // LED connected to digital pin 9 int rightOn = 8; // LED connected to digital pin 9 //establish contactor pins int leftReverse = 9; int rightReverse = 10; //The drive speed sent to the throttle int gospeed = 86; volatile int foserious = 0; //FRONT int FrontLeftSnd = 30; int FrontLeftRcv = 31; int FrontCenterSnd = 32; int FrontCenterRcv = 33; int FrontRightSnd = 34; int FrontRightRcv = 35; //RIGHT SIDE int Side1LeftSnd = 36; int Side1LeftRcv = 37; int Side1CenterSnd = 38; int Side1CenterRcv = 39; int Side1RightSnd = 40; int Side1RightRcv = 41; //BACK int BackLeftSnd = 42; int BackLeftRcv = 43; int BackCenterSnd = 44; int BackCenterRcv = 45; int BackRightSnd = 46; int BackRightRcv = 47; //LEFT SIDE int Side2LeftSnd = 49; int Side2LeftRcv = 48; int Side2CenterSnd = 51; int Side2CenterRcv = 50; int Side2RightSnd = 53; int Side2RightRcv = 52; //array of all of the input and output pin names. Used later to read all of the sensors in a for loop. int SensorOutputs[] = {FrontLeftSnd, FrontCenterSnd, FrontRightSnd, Side1LeftSnd, Side1CenterSnd, Side1RightSnd, BackLeftSnd, BackCenterSnd, BackRightSnd, Side2LeftSnd, Side2CenterSnd, Side2RightSnd}; int SensorInputs[] = {FrontLeftRcv, FrontCenterRcv, FrontRightRcv, Side1LeftRcv, Side1CenterRcv, Side1RightRcv, BackLeftRcv, BackCenterRcv, BackRightRcv, Side2LeftRcv, Side2CenterRcv, Side2RightRcv}; int dontgo = 0; //number of total sensors int SensorCount = 12; int nothingHappening = 0; int realCloseLike = 0; int goingforward = 0; int goingbackward = 0; int goingright = 0; int goingleft = 0; int backhit = 0; int fronthit = 0; int righthit = 0; int lefthit = 0; int amountToMove = 1000; int picked; void setup() { cli();//stop interrupts TCCR1A = 0;// set entire TCCR1A register to 0 TCCR1B = 0;// same for TCCR1B TCNT1 = 0;//initialize counter value to 0 // set compare match register for 1hz increments OCR1A = 512;// = (16*10^6) / (1*1024) - 1 (must be <65536) // turn on CTC mode TCCR1B |= (1 << WGM12); // Set CS10 and CS12 bits for 1024 prescaler TCCR1B |= (1 << CS12) | (1 << CS10); // enable timer compare interrupt TIMSK1 |= (1 << OCIE1A); sei();//allow interrupts Serial.begin(9600); pinMode(leftOn, OUTPUT); pinMode(rightOn, OUTPUT); pinMode(leftReverse, OUTPUT); pinMode(rightReverse, OUTPUT); //Make sure the power is off digitalWrite(leftOn, LOW); digitalWrite(rightOn, LOW); digitalWrite(leftReverse, LOW); digitalWrite(rightReverse, LOW); //bumper sensor pins pinMode(18, INPUT); pinMode(19, INPUT); //Set distance sensor output pins pinMode(FrontLeftSnd, OUTPUT); //pin 30 pinMode(FrontCenterSnd, OUTPUT); //pin 32 pinMode(FrontRightSnd, OUTPUT); // pin 34 pinMode(Side1LeftSnd, OUTPUT); //pin 36 pinMode(Side1CenterSnd, OUTPUT); //pin 38 pinMode(Side1RightSnd, OUTPUT); // pin 40 pinMode(BackLeftSnd, OUTPUT); //pin 42 pinMode(BackCenterSnd, OUTPUT); //pin 44 pinMode(BackRightSnd, OUTPUT); // pin 46 pinMode(Side2LeftSnd, OUTPUT); //pin 49 pinMode(Side2CenterSnd, OUTPUT); //pin 51 pinMode(Side2RightSnd, OUTPUT); // pin 53 //Set distance sensor input pins pinMode(FrontLeftRcv, INPUT); //pin 31 pinMode(FrontCenterRcv, INPUT); //pin 33 pinMode(FrontRightRcv, INPUT); //pin 35 pinMode(Side1LeftRcv, INPUT); //pin 37 pinMode(Side1CenterRcv, INPUT); //pin 39 pinMode(Side1RightRcv, INPUT); //pin 41 pinMode(BackLeftRcv, INPUT); //pin 43 pinMode(BackCenterRcv, INPUT); //pin 45 pinMode(BackRightRcv, INPUT); //pin 47 pinMode(Side2LeftRcv, INPUT); //pin 48 pinMode(Side2CenterRcv, INPUT); //pin 50 pinMode(Side2RightRcv, INPUT); //pin 52 //And wait a moment delay(3000); } void loop() { lookAllAround(); moveRobot(); delay(amountToMove); slowstop(); delay(1500); } ISR(TIMER1_COMPA_vect) { //Interrupt at freq of 1kHz to measure reed switch//generates pulse wave of frequency 8kHz/2 = 4kHz (takes two cycles for full wave- toggle high then toggle low) if(lefthit == 0 && righthit == 0 && fronthit == 0 && backhit == 0){ if(digitalRead(18) == HIGH){ hardstop(); Serial.println(goingbackward); Serial.println(goingforward); Serial.println(goingleft); Serial.println(goingright); Serial.println("FUCK YEAH!"); if(goingright == 1){ righthit = 1; goingright = 0; } if(goingleft == 1){ lefthit = 1; goingleft = 0; } if(goingbackward == 1){ backhit = 1; goingbackward = 0; } if(goingforward == 1){ fronthit = 1; goingforward = 0; } } } if(lefthit == 0 && righthit == 0 && fronthit == 0 && backhit == 0){ if(digitalRead(19) == HIGH){ hardstop(); Serial.println("FUCK NO!"); Serial.println(goingbackward); Serial.println(goingforward); Serial.println(goingleft); Serial.println(goingright); if(goingbackward == 1){ backhit = 1; goingbackward = 0; } if(goingforward == 1){ fronthit = 1; goingforward = 0; } if(goingright == 1){ righthit = 1; goingright = 0; } if(goingleft == 1){ lefthit = 1; goingleft = 0; } } } } void moveRobot(){ //first see if hit -- if hit while moving in any of the directions - auto-pick the other direction //-- otherwise pick randomly if(backhit == 1){ amountToMove = 1000; backhit = 0; fronthit = 0; lefthit = 0; righthit = 0; picked = 0; } else if(fronthit == 1){ amountToMove = 1000; backhit = 0; fronthit = 0; lefthit = 0; righthit = 0; picked = 1; } else if(lefthit == 1){ amountToMove = 500; backhit = 0; fronthit = 0; lefthit = 0; righthit = 0; picked = 2; } else if(righthit == 1){ amountToMove = 500; backhit = 0; fronthit = 0; lefthit = 0; righthit = 0; picked = 3; } else{ picked = random(3); Serial.println("WTF!?"); amountToMove = 1000; } switch(picked){ case 0: goingforward = 1; goingbackward = 0; goingright = 0; goingleft = 0; forwards(); Serial.println("go forwards"); break; case 1: goingforward = 0; goingbackward = 1; goingright = 0; goingleft = 0; backwards(); Serial.println("go back"); break; case 2: goingforward = 0; goingbackward = 0; goingright = 1; goingleft = 0; right(); Serial.println("go right"); break; case 3: goingforward = 0; goingbackward = 0; goingright = 0; goingleft = 1; left(); Serial.println("go left"); break; } delay(1); // delay in between reads for stability } void forwards(){ //activate the reverse contactor digitalWrite(leftReverse, HIGH); delay(50); digitalWrite(rightReverse, HIGH); delay(100); //activate the solenoids digitalWrite(leftOn, HIGH); digitalWrite(rightOn, HIGH); //take a breath delay(500); //engage the throttle analogWrite(rightThrottle, gospeed); analogWrite(leftThrottle, gospeed); } void backwards(){ //activate the solenoids digitalWrite(leftOn, HIGH); digitalWrite(rightOn, HIGH); //take a breath delay(500); // engage the throttle analogWrite(rightThrottle, gospeed); analogWrite(leftThrottle, gospeed); } void right(){ if(dontgo == 0){ //activate the solenoids digitalWrite(leftOn, HIGH); digitalWrite(rightOn, HIGH); //activate the reverse contactor digitalWrite(rightReverse, HIGH); //take a breath delay(500); //engage the throttle analogWrite(rightThrottle, gospeed); analogWrite(leftThrottle, gospeed); } //reset the variable dontgo = 0; } void left(){ if(dontgo == 0){ //activate the solenoids digitalWrite(leftOn, HIGH); digitalWrite(rightOn, HIGH); //activate the reverse contactor digitalWrite(leftReverse, HIGH); //take a breath delay(500); //engage the throttle analogWrite(rightThrottle, gospeed); analogWrite(leftThrottle, gospeed); } //reset the variable dontgo = 0; } void slowstop() { for(int fadeValue = gospeed ; fadeValue >= 0; fadeValue -=5) { // sets the value (range from 0 to 255): analogWrite(rightThrottle, fadeValue); analogWrite(leftThrottle, fadeValue); // wait for 30 milliseconds to see the dimming effect delay(500); } digitalWrite(leftOn, LOW); digitalWrite(rightOn, LOW); digitalWrite(leftReverse, LOW); digitalWrite(rightReverse, LOW); int goingforward = 0; int goingbackward = 0; int goingright = 0; int goingleft = 0; delay(2000); lookAllAround(); delay(1000); } void hardstop() { for(int fadeValue = gospeed ; fadeValue >= 0; fadeValue -=5) { // sets the value (range from 0 to 255): analogWrite(rightThrottle, fadeValue); analogWrite(leftThrottle, fadeValue); // wait for 30 milliseconds to see the dimming effect altDelay(50); } digitalWrite(leftOn, LOW); digitalWrite(rightOn, LOW); digitalWrite(leftReverse, LOW); digitalWrite(rightReverse, LOW); } void altDelay(int x) { for(unsigned int i=0; i<=x; i++) { delayMicroseconds(1000); } } void lookAllAround(){ long duration, inches, cm; for (int thisPin = 0; thisPin < 1; thisPin++) { digitalWrite(SensorOutputs[thisPin], LOW); delayMicroseconds(2); digitalWrite(SensorOutputs[thisPin], HIGH); delayMicroseconds(12); digitalWrite(SensorOutputs[thisPin], LOW); duration = pulseIn(SensorInputs[thisPin], HIGH); digitalWrite(SensorOutputs[thisPin], LOW); // convert the time into a distance inches = microsecondsToInches(duration); Serial.print(SensorOutputs[thisPin]); Serial.print(": "); Serial.print(inches); Serial.println("in, "); // //check and check again // if(inches > 10 && inches < 25){ // realCloseLike = 0; // for(int checkagain = 0; checkagain < 3; checkagain++) { // delay(100); // digitalWrite(SensorOutputs[thisPin], LOW); // delayMicroseconds(2); // digitalWrite(SensorOutputs[thisPin], HIGH); // delayMicroseconds(12); // digitalWrite(SensorOutputs[thisPin], LOW); // // duration = pulseIn(SensorInputs[thisPin], HIGH); // inches = microsecondsToInches(duration); // digitalWrite(SensorOutputs[thisPin], LOW); // // delay(100); // // if(inches > 10 && inches < 25){ // // Serial.print(SensorOutputs[thisPin]); // Serial.print(": "); // Serial.print(inches); // Serial.println("in, "); // // realCloseLike = realCloseLike + 1; // // if(realCloseLike > 2){ // dontgo = 1; // } // } // } // } //Takes about 1/2 second to check all of the sensors @ 50uS delay(100); } } long microsecondsToInches(long microseconds) { // According to Parallax's datasheet for the PING))), there are // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per // second). This gives the distance travelled by the ping, outbound // and return, so we divide by 2 to get the distance of the obstacle. // See: <a href="http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf"> <a href="http://www.parallax.com/dl/docs/prod/acc/28015-PI...</a"> http://www.parallax.com/dl/docs/prod/acc/28015-PI...</a>> return microseconds / 74 / 2; } Ejemplo de código de prueba de control del motor:
/* This example code is in the public domain. */ //establish throttle pins int leftThrottle = 3; // LED connected to digital pin 9 int rightThrottle = 5; // LED connected to digital pin 9 //establish solenoid pins int leftOn = 7; // LED connected to digital pin 9 int rightOn = 8; // LED connected to digital pin 9 //establish contactor pins int leftReverse = 9; int rightReverse = 10; //The drive speed sent to the throttle int gospeed = 82; int nothingHappening = 0; int realCloseLike = 0; int goingforward = 0; int goingbackward = 0; int goingright = 0; int goingleft = 0; int backhit = 0; int fronthit = 0; int righthit = 0; int lefthit = 0; int amountToMove = 1000; int timeToWait = 30000; int picked; void setup() { cli();//stop interrupts TCCR1A = 0;// set entire TCCR1A register to 0 TCCR1B = 0;// same for TCCR1B TCNT1 = 0;//initialize counter value to 0 // set compare match register for 1hz increments OCR1A = 512;// = (16*10^6) / (1*1024) - 1 (must be <65536) // turn on CTC mode TCCR1B |= (1 << WGM12); // Set CS10 and CS12 bits for 1024 prescaler TCCR1B |= (1 << CS12) | (1 << CS10); // enable timer compare interrupt TIMSK1 |= (1 << OCIE1A); sei();//allow interrupts Serial.begin(9600); pinMode(leftOn, OUTPUT); pinMode(rightOn, OUTPUT); pinMode(leftReverse, OUTPUT); pinMode(rightReverse, OUTPUT); //Make sure the power is off digitalWrite(leftOn, LOW); digitalWrite(rightOn, LOW); digitalWrite(leftReverse, LOW); digitalWrite(rightReverse, LOW); //bumper sensor pins pinMode(18, INPUT); pinMode(19, INPUT); //And wait a moment delay(3000); } void loop() { left(); delay(amountToMove); slowstop(); delay(timeToWait); backwards(); delay(amountToMove); slowstop(); delay(timeToWait); forwards(); delay(amountToMove); slowstop(); delay(timeToWait); right(); delay(amountToMove); slowstop(); delay(timeToWait); /// right(); delay(amountToMove); slowstop(); delay(timeToWait); forwards(); delay(amountToMove); slowstop(); delay(timeToWait); backwards(); delay(amountToMove); slowstop(); delay(timeToWait); left(); delay(amountToMove); slowstop(); delay(timeToWait); /// forwards(); delay(amountToMove); slowstop(); delay(timeToWait); backwards(); delay(amountToMove); slowstop(); delay(timeToWait); forwards(); delay(amountToMove); slowstop(); delay(timeToWait); backwards(); delay(amountToMove); slowstop(); delay(timeToWait); } ISR(TIMER1_COMPA_vect) { //Interrupt at freq of 1kHz to measure reed switch//generates pulse wave of frequency 8kHz/2 = 4kHz (takes two cycles for full wave- toggle high then toggle low) if(lefthit == 0 && righthit == 0 && fronthit == 0 && backhit == 0){ if(digitalRead(18) == HIGH){ hardstop(); Serial.println(goingbackward); Serial.println(goingforward); Serial.println(goingleft); Serial.println(goingright); Serial.println("FUCK YEAH!"); if(goingright == 1){ righthit = 1; goingright = 0; } if(goingleft == 1){ lefthit = 1; goingleft = 0; } if(goingbackward == 1){ backhit = 1; goingbackward = 0; } if(goingforward == 1){ fronthit = 1; goingforward = 0; } } } if(lefthit == 0 && righthit == 0 && fronthit == 0 && backhit == 0){ if(digitalRead(19) == HIGH){ hardstop(); Serial.println(goingbackward); Serial.println(goingforward); Serial.println(goingleft); Serial.println(goingright); Serial.println("FUCK NO!"); if(goingbackward == 1){ backhit = 1; goingbackward = 0; } if(goingforward == 1){ fronthit = 1; goingforward = 0; } if(goingright == 1){ righthit = 1; goingright = 0; } if(goingleft == 1){ lefthit = 1; goingleft = 0; } } } } void moveRobot(){ //first see if hit -- if hit while moving in any of the directions - auto-pick the other direction //-- otherwise pick randomly if(backhit == 1){ amountToMove = 1000; backhit = 0; fronthit = 0; lefthit = 0; righthit = 0; picked = 0; } else if(fronthit == 1){ amountToMove = 1000; backhit = 0; fronthit = 0; lefthit = 0; righthit = 0; picked = 1; } else if(lefthit == 1){ amountToMove = 500; backhit = 0; fronthit = 0; lefthit = 0; righthit = 0; picked = 2; } else if(righthit == 1){ amountToMove = 500; backhit = 0; fronthit = 0; lefthit = 0; righthit = 0; picked = 3; } else{ picked = random(3); Serial.println("WTF!?"); amountToMove = 1000; } switch(picked){ case 0: goingforward = 1; goingbackward = 0; goingright = 0; goingleft = 0; forwards(); Serial.println("go forwards"); break; case 1: goingforward = 0; goingbackward = 1; goingright = 0; goingleft = 0; backwards(); Serial.println("go back"); break; case 2: goingforward = 0; goingbackward = 0; goingright = 1; goingleft = 0; right(); Serial.println("go right"); break; case 3: goingforward = 0; goingbackward = 0; goingright = 0; goingleft = 1; left(); Serial.println("go left"); break; } delay(1); // delay in between reads for stability } void forwards(){ //activate the reverse contactor digitalWrite(leftReverse, HIGH); delay(50); digitalWrite(rightReverse, HIGH); delay(100); //activate the solenoids digitalWrite(leftOn, HIGH); digitalWrite(rightOn, HIGH); //take a breath delay(500); //engage the throttle analogWrite(rightThrottle, gospeed); analogWrite(leftThrottle, gospeed); } void backwards(){ //activate the solenoids digitalWrite(leftOn, HIGH); digitalWrite(rightOn, HIGH); //take a breath delay(500); // engage the throttle analogWrite(rightThrottle, gospeed); analogWrite(leftThrottle, gospeed); } void right(){ //activate the solenoids digitalWrite(leftOn, HIGH); digitalWrite(rightOn, HIGH); //activate the reverse contactor digitalWrite(rightReverse, HIGH); //take a breath delay(500); //engage the throttle analogWrite(rightThrottle, gospeed); analogWrite(leftThrottle, gospeed); } void left(){ //activate the solenoids digitalWrite(leftOn, HIGH); digitalWrite(rightOn, HIGH); //activate the reverse contactor digitalWrite(leftReverse, HIGH); //take a breath delay(500); //engage the throttle analogWrite(rightThrottle, gospeed); analogWrite(leftThrottle, gospeed); } void slowstop() { for(int fadeValue = gospeed ; fadeValue >= 0; fadeValue -=5) { // sets the value (range from 0 to 255): analogWrite(rightThrottle, fadeValue); analogWrite(leftThrottle, fadeValue); // wait for 30 milliseconds to see the dimming effect delay(500); } digitalWrite(leftOn, LOW); digitalWrite(rightOn, LOW); digitalWrite(leftReverse, LOW); digitalWrite(rightReverse, LOW); int goingforward = 0; int goingbackward = 0; int goingright = 0; int goingleft = 0; } void hardstop() { for(int fadeValue = gospeed ; fadeValue >= 0; fadeValue -=5) { // sets the value (range from 0 to 255): analogWrite(rightThrottle, fadeValue); analogWrite(leftThrottle, fadeValue); // wait for 30 milliseconds to see the dimming effect altDelay(50); } digitalWrite(leftOn, LOW); digitalWrite(rightOn, LOW); digitalWrite(leftReverse, LOW); digitalWrite(rightReverse, LOW); } void altDelay(int x) { for(unsigned int i=0; i<=x; i++) { delayMicroseconds(1000); } }
