MicroMouse / micromouse4_encoder_testing

mm4_encoder_testing

Dependencies:   QEI mbed-src-AV

main.cpp@1:1fed86c424fd, 2015-11-08 (annotated)

Committer:
jliu
Date:
Sun Nov 08 01:35:36 2015 +0000
Revision:
1:1fed86c424fd
Parent:
0:c1b3cc00b091
Child:
2:664717ad13cc
can do a zig-zag now

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kelvincheng 0:c1b3cc00b091 1 // Micromouse 2
kelvincheng 0:c1b3cc00b091 2 // Further Test out the motors.
kelvincheng 0:c1b3cc00b091 3 // Control with PWM.
kelvincheng 0:c1b3cc00b091 4 // Test Serial Output.
kelvincheng 0:c1b3cc00b091 5
kelvincheng 0:c1b3cc00b091 6
kelvincheng 0:c1b3cc00b091 7 #include "mbed.h"
kelvincheng 0:c1b3cc00b091 8 #include "PinDefinitions.h"
kelvincheng 0:c1b3cc00b091 9 Serial pc(PC_6, PC_7); // These pins work for serial communications.
kelvincheng 0:c1b3cc00b091 10 InterruptIn mybutton(USER_BUTTON);
kelvincheng 0:c1b3cc00b091 11 DigitalOut led3(LED3);
kelvincheng 0:c1b3cc00b091 12 DigitalOut motor_enabler(PB_6);
kelvincheng 0:c1b3cc00b091 13
kelvincheng 0:c1b3cc00b091 14 int Rdistance() { return RENC.getPulses(); }
kelvincheng 0:c1b3cc00b091 15 int Ldistance() { return -LENC.getPulses(); }
kelvincheng 0:c1b3cc00b091 16
kelvincheng 0:c1b3cc00b091 17
kelvincheng 0:c1b3cc00b091 18 void setLeftPwm(float speed) {
kelvincheng 0:c1b3cc00b091 19 if (speed == 0) {
kelvincheng 0:c1b3cc00b091 20 LMOTORA = 1.0;
kelvincheng 0:c1b3cc00b091 21 LMOTORB = 1.0;
kelvincheng 0:c1b3cc00b091 22 }
kelvincheng 0:c1b3cc00b091 23
kelvincheng 0:c1b3cc00b091 24 if (speed > 0) {
kelvincheng 0:c1b3cc00b091 25 LMOTORA = speed;
kelvincheng 0:c1b3cc00b091 26 LMOTORB = 0;
kelvincheng 0:c1b3cc00b091 27 }
kelvincheng 0:c1b3cc00b091 28 else {
kelvincheng 0:c1b3cc00b091 29 LMOTORA = 0;
kelvincheng 0:c1b3cc00b091 30 LMOTORB = -speed;
kelvincheng 0:c1b3cc00b091 31 }
kelvincheng 0:c1b3cc00b091 32 }
kelvincheng 0:c1b3cc00b091 33
kelvincheng 0:c1b3cc00b091 34 void setRightPwm(float speed) {
kelvincheng 0:c1b3cc00b091 35 if (speed == 0) {
kelvincheng 0:c1b3cc00b091 36 RMOTORA = 1.0;
kelvincheng 0:c1b3cc00b091 37 RMOTORB = 1.0;
kelvincheng 0:c1b3cc00b091 38 }
kelvincheng 0:c1b3cc00b091 39
kelvincheng 0:c1b3cc00b091 40 if (speed > 0) {
kelvincheng 0:c1b3cc00b091 41 RMOTORA = speed;
kelvincheng 0:c1b3cc00b091 42 RMOTORB = 0;
kelvincheng 0:c1b3cc00b091 43 }
kelvincheng 0:c1b3cc00b091 44 else {
kelvincheng 0:c1b3cc00b091 45 RMOTORA = 0;
kelvincheng 0:c1b3cc00b091 46 RMOTORB = -speed;
kelvincheng 0:c1b3cc00b091 47 }
kelvincheng 0:c1b3cc00b091 48 }
kelvincheng 0:c1b3cc00b091 49
kelvincheng 0:c1b3cc00b091 50 void pressed()
kelvincheng 0:c1b3cc00b091 51 {
kelvincheng 0:c1b3cc00b091 52 led3 = !led3;
kelvincheng 0:c1b3cc00b091 53 pc.printf("Hellooooo\r\n");
kelvincheng 0:c1b3cc00b091 54 }
kelvincheng 0:c1b3cc00b091 55
jliu 1:1fed86c424fd 56
jliu 1:1fed86c424fd 57 void goForward(int dist)
jliu 1:1fed86c424fd 58 {
jliu 1:1fed86c424fd 59 setRightPwm(0.5);
jliu 1:1fed86c424fd 60 setLeftPwm(0.45);
jliu 1:1fed86c424fd 61 while (Rdistance() < dist && Ldistance() < dist)
jliu 1:1fed86c424fd 62 {
jliu 1:1fed86c424fd 63 pc.printf("Lp: %d\nRp: %d\n\n", Ldistance(), Rdistance());
jliu 1:1fed86c424fd 64 }
jliu 1:1fed86c424fd 65 setRightPwm(0);
jliu 1:1fed86c424fd 66 setLeftPwm(0);
jliu 1:1fed86c424fd 67 RENC.reset();
jliu 1:1fed86c424fd 68 LENC.reset();
jliu 1:1fed86c424fd 69 }
jliu 1:1fed86c424fd 70
jliu 1:1fed86c424fd 71 void turnR()
jliu 1:1fed86c424fd 72 {
jliu 1:1fed86c424fd 73 const int turn_dist = 1000;
jliu 1:1fed86c424fd 74
jliu 1:1fed86c424fd 75 setLeftPwm(0.5);
jliu 1:1fed86c424fd 76 while(Ldistance() < turn_dist)
jliu 1:1fed86c424fd 77 {
jliu 1:1fed86c424fd 78 pc.printf("Turning... Lp: %d\n", Ldistance());
jliu 1:1fed86c424fd 79 }
jliu 1:1fed86c424fd 80 setLeftPwm(0);
jliu 1:1fed86c424fd 81 RENC.reset();
jliu 1:1fed86c424fd 82 LENC.reset();
jliu 1:1fed86c424fd 83 }
jliu 1:1fed86c424fd 84
jliu 1:1fed86c424fd 85 void turnL()
jliu 1:1fed86c424fd 86 {
jliu 1:1fed86c424fd 87 const int turn_dist = 1000;
jliu 1:1fed86c424fd 88
jliu 1:1fed86c424fd 89 setRightPwm(0.5);
jliu 1:1fed86c424fd 90 while(Rdistance() < turn_dist)
jliu 1:1fed86c424fd 91 {
jliu 1:1fed86c424fd 92 pc.printf("Turning... Rp: %d\n", Rdistance());
jliu 1:1fed86c424fd 93 }
jliu 1:1fed86c424fd 94 setRightPwm(0);
jliu 1:1fed86c424fd 95 RENC.reset();
jliu 1:1fed86c424fd 96 LENC.reset();
jliu 1:1fed86c424fd 97 }
jliu 1:1fed86c424fd 98
kelvincheng 0:c1b3cc00b091 99 int main() {
kelvincheng 0:c1b3cc00b091 100 mybutton.fall(&pressed);
kelvincheng 0:c1b3cc00b091 101 led3 = 0;
kelvincheng 0:c1b3cc00b091 102 motor_enabler = 1;
kelvincheng 0:c1b3cc00b091 103
jliu 1:1fed86c424fd 104 goForward(1400);
jliu 1:1fed86c424fd 105 //wait(1);
jliu 1:1fed86c424fd 106 turnR();
jliu 1:1fed86c424fd 107 //wait(1);
jliu 1:1fed86c424fd 108 goForward(700);
jliu 1:1fed86c424fd 109 turnL();
jliu 1:1fed86c424fd 110 goForward(1200);
jliu 1:1fed86c424fd 111
kelvincheng 0:c1b3cc00b091 112 pc.printf("Done.\n");
kelvincheng 0:c1b3cc00b091 113 }