MicroMouse / micromouse4_encoder_testing

mm4_encoder_testing

Dependencies:   QEI mbed-src-AV

main.cpp@2:664717ad13cc, 2015-11-11 (annotated)

Committer:
jliu
Date:
Wed Nov 11 00:23:33 2015 +0000
Revision:
2:664717ad13cc
Parent:
1:1fed86c424fd 
k

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