// BBC-Buggy // TAB FILE > movement.ino // MIND - updatePosition() // SKILL - t_light(), t_dark(), t_atLight(), t_atDark() // t_unwind() // MOVEMENT - forward(), backward(), turnRight(), turnLeft(), // PenUp(), PenDown() // Arduino Forum // 1 Radian = 57.2957795 degrees. To avoid calculating decimals with arduino, use something like the following: // deg = rad * 57296 / 1000 // rad = deg * 1000 / 57296 // radians = (degrees * 71) / 4068 // degrees = (radians * 4068) / 71 //== NEW ==================================================== /* */ //== CHANGED ================================================ /* */ //== MIND =================================================== void go_Home() // [H], [g][H] { t_toHome(); homeDistance =sqrt( sq(AtY) + sq(AtX) ); p(" gH_homeDistance",homeDistance); thisMove =homeDistance; go_Forward(); } //---------------------------- void t_toHome() // [t][H] { p(" tohome",0); p("(-AtY) /(-AtX)=",(-AtY) /(-AtX)); homeHeading =180 +(4068 /71 *atan( (-AtY) /(-AtX) ));p(" hmhead",homeHeading); thisMove =homeHeading -heading; if (thisMove >180) { thisMove =thisMove -360; } p(" tohome_thisMove",thisMove); go_Turn(); } //---------------------------- void t_unwind() // [t][u] { if (heading < 0) {thisMove =-heading; go_turnLeft(); } else { if (heading > 0) {thisMove =heading; go_turnRight(); } } } //---------------------------- void updatePosition() { p(" heading",heading); float head =heading; moveX =thisMove *cos((head * 71) / 4068); p(" X",moveX); moveY =thisMove *sin((head * 71) / 4068); p(" Y",moveY); AtX +=moveX; AtY +=moveY; } //== GO =========================== void go_Turn() { if (thisMove >0) { go_turnLeft(); } else { if (thisMove <0) { thisMove =-thisMove; go_turnRight(); } } } void go_Forward() { Forward(); updatePosition(); } void go_Backward() { Backward(); thisMove =-thisMove; updatePosition(); } void go_turnRight() { turnRight(); heading =heading -thisMove; } void go_turnLeft() { turnLeft(); heading =heading +thisMove; } //== SKILL ================================================== void t_light() { if (analogRead(Leye) > analogRead(Reye)) { turnLeft(); } else { turnRight(); } } //---------------------------- void t_dark() { if (analogRead(Leye) < analogRead(Reye)) { turnLeft(); } else { turnRight(); } } //---------------------------- void t_atLight() { if (analogRead(Leye) > analogRead(Reye)) { do { turnLeft(); if (Quit()) { break; } } while (analogRead(Leye) > analogRead(Reye)); } else { do { turnRight(); if (Quit()) { break; } } while (analogRead(Leye) < analogRead(Reye)); } } //---------------------------- void t_atDark() { if (analogRead(Leye) < analogRead(Reye)) { do { turnLeft(); if (Quit()) { break; } } while (analogRead(Leye) < analogRead(Reye)); } else { do { turnRight(); if (Quit()) { break; } } while (analogRead(Leye) > analogRead(Reye)); } } //== PEN ==================================================== void PenUp() { digitalWrite(PenCtrl,HIGH); } //---------------------------- void PenDown() { digitalWrite(PenCtrl,LOW); } //== MOVEMENT =============================================== void Forward() // move steps forwards { digitalWrite(MotorsOff, LOW); //motors ON digitalWrite(LMdir, LOW); //Fd digitalWrite(RMdir, LOW); //Fd steps =thisMove *stepScale100 /100; msteps =0; do { digitalWrite(Mstep, HIGH); delay(steptime); digitalWrite(Mstep, LOW); delay(steptime); msteps +=1; if(Quit()){break;} } while (msteps < steps); digitalWrite(MotorsOff, HIGH); //motors OFF thisMove =msteps *100 /stepScale100; } //---------------------------- void Backward() { digitalWrite(MotorsOff, LOW); //motors ON digitalWrite(LMdir, HIGH); //Bk digitalWrite(RMdir, HIGH); //Bk steps =thisMove *stepScale100 /100; msteps =0; do { digitalWrite(Mstep, HIGH); delay(steptime); digitalWrite(Mstep, LOW); delay(steptime); msteps +=1; if(Quit()){break;} } while (msteps < steps); digitalWrite(MotorsOff, HIGH); //motors OFF thisMove =msteps *100 /stepScale100; } //---------------------------- void turnRight() { Serial.print("right"); Serial.print(thisMove); digitalWrite(MotorsOff, LOW); //motors ON digitalWrite(LMdir, LOW); //Fd digitalWrite(RMdir, HIGH); //Bk steps =thisMove *rotateScale360 /360; msteps =0; do { digitalWrite(Mstep, HIGH); delay(steptime); digitalWrite(Mstep, LOW); delay(steptime); msteps +=1; if(Quit()){break;} } while (msteps < steps); digitalWrite(MotorsOff, HIGH); //motors OFF f_TurnedRight =true; thisMove =msteps *360 /rotateScale360; } //---------------------------- void turnLeft() { Serial.print("left"); digitalWrite(MotorsOff, LOW); //motors ON digitalWrite(LMdir, HIGH); //Bk digitalWrite(RMdir, LOW); //Fd steps =thisMove *rotateScale360 /360; msteps =0; do { digitalWrite(Mstep, HIGH); delay(steptime); digitalWrite(Mstep, LOW); delay(steptime); msteps +=1; if(Quit()){break;} } while (msteps < steps); digitalWrite(MotorsOff, HIGH); //motors OFF f_TurnedRight =false; thisMove =msteps *360 /rotateScale360; } //=========================================================== //void p(int stuff) //{Serial.print(" ");Serial.print(stuff);} void p(String s_stuff1,int i_stuff2) {Serial.print(s_stuff1);Serial.println(i_stuff2);} /* //== PINS =================================================== //pin0 Rx when connected to a SparkFun RS232 module can't have // a 1k protection resistor as doesn't then receive. //pin1 Tx //pin2 //pin3 */ //===========================================================