diff --git a/mecanum.ino b/mecanum.ino index 872fc7f..452ee1c 100644 --- a/mecanum.ino +++ b/mecanum.ino @@ -1,21 +1,47 @@ -#include +//#include #include -#include -#include +//#include +//#include #define IR 4 IRrecv irrecv(IR); +//GPIO Wheels #define WHEEL_FRONT_LEFT_F A0 #define WHEEL_REAR_LEFT_R A1 #define WHEEL_REAR_RIGHT_F A2 #define WHEEL_FRONT_RIGHT_R A3 - -#define WHEEL_FRONT_LEFT_R 10 -#define WHEEL_REAR_LEFT_F 9 -#define WHEEL_FRONT_RIGHT_F 8 #define WHEEL_REAR_RIGHT_R 7 +#define WHEEL_FRONT_RIGHT_F 8 +#define WHEEL_REAR_LEFT_F 9 +#define WHEEL_FRONT_LEFT_R 10 -/* +//Response from Remote +#define RFORWARD 4060792887 +#define RBACKWARD 4060752087 +#define RSTRAFE_LEFT 4060768407 +#define RSTRAFE_RIGHT 4060784727 +#define RSTOP 4060801047 +#define RTURN_LEFT 4060750047 +#define RTURN_RIGHT 4060790847 +#define RDIAG_LEFT_FOR 4060786767 +#define RDIAG_RIGHT_BACK 4060778607 +#define RDIAG_RIGHT_FOR 4060782687 +#define RDIAG_LEFT_BACK 4060766367 + +//direction bitmaps +#define DSTOP 0b00000000 +#define DFORWARD 0b11110000 +#define DBACKWARD 0b00001111 +#define DSTRAFE_LEFT 0b01101001 +#define DSTRAFE_RIGHT 0b10010110 +#define DDIAG_LEFT_FOR 0b01100000 +#define DDIAG_RIGHT_BACK 0b00000110 +#define DDIAG_RIGHT_FOR 0b10010000 +#define DDIAG_LEFT_BACK 0b00001001 +#define DTURN_LEFT 0b10100101 +#define DTURN_RIGHT 0b01011010 + +/* All Responses * 4060792887 ^ * 4060752087 v * 4060768407 < @@ -56,80 +82,19 @@ void setup() { pinMode(WHEEL_FRONT_LEFT_F, OUTPUT); } +void drive(int direction) { + digitalWrite(WHEEL_FRONT_LEFT_F, direction & 0b10000000 ? LOW : HIGH); + digitalWrite(WHEEL_FRONT_RIGHT_F, direction & 0b01000000 ? LOW : HIGH); + digitalWrite(WHEEL_REAR_LEFT_F, direction & 0b00100000 ? LOW : HIGH); + digitalWrite(WHEEL_REAR_RIGHT_F, direction & 0b00010000 ? LOW : HIGH); -void stop(){ - digitalWrite(WHEEL_REAR_RIGHT_R, HIGH); - digitalWrite(WHEEL_REAR_RIGHT_F, HIGH); - digitalWrite(WHEEL_REAR_LEFT_R, HIGH); - digitalWrite(WHEEL_REAR_LEFT_F, HIGH); - digitalWrite(WHEEL_FRONT_RIGHT_R, HIGH); - digitalWrite(WHEEL_FRONT_RIGHT_F, HIGH); - digitalWrite(WHEEL_FRONT_LEFT_R, HIGH); - digitalWrite(WHEEL_FRONT_LEFT_F, HIGH); -} - -void forward(){ - digitalWrite(WHEEL_REAR_RIGHT_F, LOW); - digitalWrite(WHEEL_REAR_LEFT_F, LOW); - digitalWrite(WHEEL_FRONT_RIGHT_F, LOW); - digitalWrite(WHEEL_FRONT_LEFT_F, LOW); -} - -void backward(){ - digitalWrite(WHEEL_REAR_RIGHT_R, LOW); - digitalWrite(WHEEL_REAR_LEFT_R, LOW); - digitalWrite(WHEEL_FRONT_RIGHT_R, LOW); - digitalWrite(WHEEL_FRONT_LEFT_R, LOW); -} - -void strafe_l(){ - digitalWrite(WHEEL_REAR_RIGHT_R, LOW); - digitalWrite(WHEEL_REAR_LEFT_F, LOW); - digitalWrite(WHEEL_FRONT_RIGHT_F, LOW); - digitalWrite(WHEEL_FRONT_LEFT_R, LOW); -} - -void strafe_r(){ - digitalWrite(WHEEL_REAR_RIGHT_F, LOW); - digitalWrite(WHEEL_REAR_LEFT_R, LOW); - digitalWrite(WHEEL_FRONT_RIGHT_R, LOW); - digitalWrite(WHEEL_FRONT_LEFT_F, LOW); -} - -void diag_l_f(){ - digitalWrite(WHEEL_REAR_LEFT_F, LOW); - digitalWrite(WHEEL_FRONT_RIGHT_F, LOW); -} - -void diag_r_r(){ - digitalWrite(WHEEL_REAR_LEFT_R, LOW); - digitalWrite(WHEEL_FRONT_RIGHT_R, LOW); -} - -void diag_r_f(){ - digitalWrite(WHEEL_REAR_RIGHT_F, LOW); - digitalWrite(WHEEL_FRONT_LEFT_F, LOW); -} - -void diag_l_r(){ - digitalWrite(WHEEL_REAR_RIGHT_R, LOW); - digitalWrite(WHEEL_FRONT_LEFT_R, LOW); -} - -void turn_l(){ - digitalWrite(WHEEL_REAR_RIGHT_R, LOW); - digitalWrite(WHEEL_REAR_LEFT_F, LOW); - digitalWrite(WHEEL_FRONT_RIGHT_R, LOW); - digitalWrite(WHEEL_FRONT_LEFT_F, LOW); -} - -void turn_r(){ - digitalWrite(WHEEL_REAR_RIGHT_F, LOW); - digitalWrite(WHEEL_REAR_LEFT_R, LOW); - digitalWrite(WHEEL_FRONT_RIGHT_F, LOW); - digitalWrite(WHEEL_FRONT_LEFT_R, LOW); + digitalWrite(WHEEL_FRONT_LEFT_R, direction & 0b00001000 ? LOW : HIGH); + digitalWrite(WHEEL_FRONT_RIGHT_R, direction & 0b00000100 ? LOW : HIGH); + digitalWrite(WHEEL_REAR_LEFT_R, direction & 0b00000010 ? LOW : HIGH); + digitalWrite(WHEEL_REAR_RIGHT_R, direction & 0b00000001 ? LOW : HIGH); } +//MIK IR Remote.pdf S.12 I. Kübler unsigned long getIRremote(){ static unsigned long lastValue = 0; decode_results irResults; @@ -146,66 +111,75 @@ unsigned long getIRremote(){ void loop() { unsigned long remote = getIRremote(); - static unsigned long modus; + static unsigned long mode; + static unsigned long lastMode = 0; if(remote){ Serial.println(remote); - modus = remote; + mode = remote; + if(mode != lastMode){ + drive(DSTOP); + delay(50); + lastMode = mode; + } } - if(modus == 4060792887){ - forward(); - }else if(modus == 4060801047){ - stop(); - }else if(modus == 4060752087){ - backward(); - }else if(modus == 4060768407){ - strafe_l(); - }else if(modus == 4060784727){ - strafe_r(); - }else if(modus == 4060750047){ - turn_l(); - }else if(modus == 4060790847){ - turn_r(); - }else if(modus == 4060786767){ - diag_l_f(); - }else if(modus == 4060778607){ - diag_r_r(); - }else if(modus == 4060782687){ - diag_r_f(); - }else if(modus == 4060766367){ - diag_l_r(); + switch(mode){ + case RFORWARD: + drive(DFORWARD); + break; + case RSTOP: + drive(DSTOP); + break; + case RBACKWARD: + drive(DBACKWARD); + break; + case RSTRAFE_LEFT: + drive(DSTRAFE_LEFT); + break; + case RSTRAFE_RIGHT: + drive(DSTRAFE_RIGHT); + break; + case RTURN_LEFT: + drive(DTURN_LEFT); + break; + case RTURN_RIGHT: + drive(DTURN_RIGHT); + break; + case RDIAG_LEFT_FOR: + drive(DDIAG_LEFT_FOR); + break; + case RDIAG_LEFT_BACK: + drive(DDIAG_LEFT_BACK); + break; + case RDIAG_RIGHT_FOR: + drive(DDIAG_RIGHT_FOR); + break; + case RDIAG_RIGHT_BACK: + drive(DDIAG_RIGHT_BACK); + break; } - /* - stop(); - forward(); - delay(1000); - stop(); - backward(); - delay(1000); - stop(); - strafe_l(); - delay(1000); - stop(); - strafe_r(); - delay(1000); - stop(); - diag_l_f(); - delay(1000); - stop(); - diag_l_r(); - delay(1000); - stop(); - diag_r_f(); - delay(1000); - stop(); - diag_r_r(); - delay(1000); - stop(); - turn_l(); - delay(1000); - stop(); - turn_r(); - delay(1000); - stop(); + if(mode == RFORWARD){ + drive(DFORWARD); + }else if(mode == RSTOP){ + drive(DSTOP); + }else if(mode == RBACKWARD){ + drive(DBACKWARD); + }else if(mode == RSTRAFE_LEFT){ + drive(DSTRAFE_LEFT); + }else if(mode == RSTRAFE_RIGHT){ + drive(DSTRAFE_RIGHT); + }else if(mode == RTURN_LEFT){ + drive(DTURN_LEFT); + }else if(mode == RTURN_RIGHT){ + drive(DTURN_RIGHT); + }else if(mode == RDIAG_LEFT_FOR){ + drive(DDIAG_LEFT_FOR); + }else if(mode == RDIAG_RIGHT_BACK){ + drive(DDIAG_RIGHT_BACK); + }else if(mode == RDIAG_RIGHT_FOR){ + drive(DDIAG_RIGHT_FOR); + }else if(mode == RDIAG_LEFT_BACK){ + drive(DDIAG_LEFT_BACK); + } */ }