// Zeaker2 // TAB FILE > movement.ino // MOVEMENT - forward(), backward(), turnRight(), turnLeft(), // ForwardLeft(), ForwardRight() // BackwardLeft(), BackwardRight() // PenUp(), PenDown() //== NEW ==================================================== /* */ //== CHANGED ================================================ /* */ //=========================================================== //== PEN ==================================================== void PenUp() { //do 4 steps 6 times =24 steps for half a revolution //then do 4 more steps to hit the cam stop digitalWrite(relayPin,HIGH); msteps =0; do { tellZ_addr_data(1,1); delay(pensteptime); tellZ_addr_data(1,0); delay(pensteptime); tellZ_addr_data(1,2); delay(pensteptime); tellZ_addr_data(1,3); delay(pensteptime); msteps +=1; } while (msteps < 7); } //---------------------------- void PenDown() { //do 4 steps 6 times =24 steps for half a revolution //then do 4 more steps to hit the cam stop digitalWrite(relayPin,HIGH); msteps =0; do { tellZ_addr_data(1,2); delay(pensteptime); tellZ_addr_data(1,0); delay(pensteptime); tellZ_addr_data(1,1); delay(pensteptime); tellZ_addr_data(1,3); delay(pensteptime); msteps +=1; } while (msteps < 7); } //---------------------------- void PenInitialise() { Serial.println(); Serial.println("hit 'Q' or '.' when Pen is UP"); digitalWrite(relayPin,HIGH); while(true) { tellZ_addr_data(1,1); delay(pensteptime *2); tellZ_addr_data(1,0); delay(pensteptime *2); tellZ_addr_data(1,2); delay(pensteptime *2); tellZ_addr_data(1,3); delay(pensteptime *2); if(Quit()) { break; } } // end while f_penStateUp =true; digitalWrite(relayPin,LOW); Serial.println("Pen set to UP"); } //== MOVEMENT =============================================== void Forward() // move steps forwards { digitalWrite(relayPin,HIGH); msteps =0; do { FD2(); } while (msteps < steps); } //---------------------------- void Backward() { digitalWrite(relayPin,HIGH); msteps =0; do { BK2(); } while (msteps < steps); } //---------------------------- void turnRight() { digitalWrite(relayPin,HIGH); msteps =0; do { RT2(); } while (msteps < steps); } //---------------------------- void turnLeft() { digitalWrite(relayPin,HIGH); msteps =0; do { LT2(); } while (msteps < steps); } //---------------------------- void FD2() //FD two steps, ie one mm { //right motor address D1,D0 //left motor address D3,D2 RmotorAt +=1; RmotorAt =RmotorAt %4; LmotorAt +=1; LmotorAt =LmotorAt %4; tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; RmotorAt +=1; RmotorAt =RmotorAt %4; LmotorAt +=1; LmotorAt =LmotorAt %4; tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; } //---------------------------- void BK2() //BK two steps, ie one mm { //right motor address 0 //left motor address 1 RmotorAt -=1; RmotorAt =min(RmotorAt,3); //0-1=>255 so min to 3 LmotorAt -=1; LmotorAt =min(LmotorAt,3); //0-1=>255 so min to 3 tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; RmotorAt -=1; RmotorAt =min(RmotorAt,3); //0-1=>255 so min to 3 LmotorAt -=1; LmotorAt =min(LmotorAt,3); //0-1=>255 so min to 3 tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; } //---------------------------- void RT2() //RT two steps, ie one degree { //right motor address 0 //left motor address 1 RmotorAt -=1; RmotorAt =min(RmotorAt,3); //0-1=>255 so min to 3 LmotorAt +=1; LmotorAt =LmotorAt %4; tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; RmotorAt -=1; RmotorAt =min(RmotorAt,3); //0-1=>255 so min to 3 LmotorAt +=1; LmotorAt =LmotorAt %4; tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; } //---------------------------- void LT2() //RT two steps, ie one degree { //right motor address 0 //left motor address 1 RmotorAt +=1; RmotorAt =RmotorAt %4; LmotorAt -=1; LmotorAt =min(LmotorAt,3); //0-1=>255 so min to 3 tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; RmotorAt +=1; RmotorAt =RmotorAt %4; LmotorAt -=1; LmotorAt =min(LmotorAt,3); //0-1=>255 so min to 3 tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; } //---------------------------- void ForwardRight() //turn towards right by using left motor { digitalWrite(relayPin,HIGH); msteps =0; do { FR2(); } while (msteps < steps); } //---------------------------- void ForwardLeft() //turn towards left by using right motor { digitalWrite(relayPin,HIGH); msteps =0; do { FL2(); } while (msteps < steps); } //---------------------------- void BackwardRight() { digitalWrite(relayPin,HIGH); msteps =0; do { BR2(); } while (msteps < steps); } //---------------------------- void BackwardLeft() { digitalWrite(relayPin,HIGH); msteps =0; do { BL2(); } while (msteps < steps); } //---------------------------- void FR2() { //left motor D3,D2 LmotorAt +=1; LmotorAt =LmotorAt %4; tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; LmotorAt +=1; LmotorAt =LmotorAt %4; tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; } //---------------------------- void FL2() { //right motor D1,D0 RmotorAt +=1; RmotorAt =RmotorAt %4; tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; RmotorAt +=1; RmotorAt =RmotorAt %4; tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; } //---------------------------- void BR2() { //left motor D3,D2 LmotorAt -=1; LmotorAt =min(LmotorAt,3); //0-1=>255 so min to 3 tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; LmotorAt -=1; LmotorAt =min(LmotorAt,3); //0-1=>255 so min to 3 tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; } //---------------------------- void BL2() { //right motor D1,D0 RmotorAt -=1; RmotorAt =min(RmotorAt,3); //0-1=>255 so min to 3 tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; RmotorAt -=1; RmotorAt =min(RmotorAt,3); //0-1=>255 so min to 3 tellZ_addr_data(0,(mCode[LmotorAt]<<2) +mCode[RmotorAt]); delay(steptime); msteps +=1; } //---------------------------- void tellZ_addr_data(byte Zaddr,byte Zdata) { Zserial.write((Zaddr <<4) +Zdata); } //===========================================================