MicroMouse / micromouse4_encoder_testing

mm4_encoder_testing

Dependencies:   QEI mbed-src-AV

main.cpp@0:c1b3cc00b091, 2015-11-06 (annotated)

Committer:
kelvincheng
Date:
Fri Nov 06 01:28:38 2015 +0000
Revision:
0:c1b3cc00b091
Child:
1:1fed86c424fd
mm4_encoder_testing

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
kelvincheng 0:c1b3cc00b091 56 int main() {
kelvincheng 0:c1b3cc00b091 57 mybutton.fall(&pressed);
kelvincheng 0:c1b3cc00b091 58 led3 = 0;
kelvincheng 0:c1b3cc00b091 59 motor_enabler = 1;
kelvincheng 0:c1b3cc00b091 60 pc.printf("\n\n\nHello world\n");
kelvincheng 0:c1b3cc00b091 61
kelvincheng 0:c1b3cc00b091 62 /*
kelvincheng 0:c1b3cc00b091 63 while (Rdistance() < 5000) { //while encoder pulses are less than 5000, turn led3 on and set LMOTORA to 0.50
kelvincheng 0:c1b3cc00b091 64 pc.printf("Pulse Count: %d\n", Rdistance());
kelvincheng 0:c1b3cc00b091 65 led3 = 1;
kelvincheng 0:c1b3cc00b091 66 setRightPwm(0.5);
kelvincheng 0:c1b3cc00b091 67 }
kelvincheng 0:c1b3cc00b091 68 RENC.reset(); // reset encoder pulse count to zero
kelvincheng 0:c1b3cc00b091 69 while (Rdistance() < 5000) { //while encoder pulses are less than 5000, turn led3 off and set LMOTORA to 0.25
kelvincheng 0:c1b3cc00b091 70 pc.printf("Pulse Count: %d\n", Rdistance());
kelvincheng 0:c1b3cc00b091 71 led3 = 0;
kelvincheng 0:c1b3cc00b091 72 setRightPwm(0.25);
kelvincheng 0:c1b3cc00b091 73 }
kelvincheng 0:c1b3cc00b091 74 RENC.reset();
kelvincheng 0:c1b3cc00b091 75 while (Rdistance() < 10000) { //while encoder pulses are less than 10000, turn led3 on and set LMOTORA to 1.00
kelvincheng 0:c1b3cc00b091 76 pc.printf("Pulse Count: %d\n", Rdistance());
kelvincheng 0:c1b3cc00b091 77 led3 = 1;
kelvincheng 0:c1b3cc00b091 78 setRightPwm(1.0);
kelvincheng 0:c1b3cc00b091 79 }
kelvincheng 0:c1b3cc00b091 80 setRightPwm(0.0);
kelvincheng 0:c1b3cc00b091 81 */
kelvincheng 0:c1b3cc00b091 82
kelvincheng 0:c1b3cc00b091 83
kelvincheng 0:c1b3cc00b091 84
kelvincheng 0:c1b3cc00b091 85 while (Ldistance() < 5000) { //while encoder pulses are less than 5000, turn led3 on and set LMOTORA to 0.50
kelvincheng 0:c1b3cc00b091 86 pc.printf("Pulse Count: %d\n", Ldistance());
kelvincheng 0:c1b3cc00b091 87 led3 = 1;
kelvincheng 0:c1b3cc00b091 88 setLeftPwm(0.5);
kelvincheng 0:c1b3cc00b091 89 }
kelvincheng 0:c1b3cc00b091 90 LENC.reset(); // reset encoder pulse count to zero
kelvincheng 0:c1b3cc00b091 91 while (Ldistance() < 5000) { //while encoder pulses are less than 5000, turn led3 off and set LMOTORA to 0.25
kelvincheng 0:c1b3cc00b091 92 pc.printf("Pulse Count: %d\n", Ldistance());
kelvincheng 0:c1b3cc00b091 93 led3 = 0;
kelvincheng 0:c1b3cc00b091 94 setLeftPwm(0.25);
kelvincheng 0:c1b3cc00b091 95 }
kelvincheng 0:c1b3cc00b091 96 LENC.reset();
kelvincheng 0:c1b3cc00b091 97 while (Ldistance() < 10000) { //while encoder pulses are less than 10000, turn led3 on and set LMOTORA to 1.00
kelvincheng 0:c1b3cc00b091 98 pc.printf("Pulse Count: %d\n", Ldistance());
kelvincheng 0:c1b3cc00b091 99 led3 = 1;
kelvincheng 0:c1b3cc00b091 100 setLeftPwm(1.0);
kelvincheng 0:c1b3cc00b091 101 }
kelvincheng 0:c1b3cc00b091 102 setLeftPwm(0.0);
kelvincheng 0:c1b3cc00b091 103
kelvincheng 0:c1b3cc00b091 104 pc.printf("Done.\n");
kelvincheng 0:c1b3cc00b091 105 }