Garrett Leach / Mbed 2 deprecated MagicianWithBluetoothandEncoder

Allows Magician Robot to run with Android Bluetooth app and wheel encoders to make wheel turn at the same time.

Dependencies:   Motordriver mbed

main.cpp@0:10883d5ab141, 2014-10-19 (annotated)

Committer:
Garr12100
Date:
Sun Oct 19 02:29:13 2014 +0000
Revision:
0:10883d5ab141
Allows Magician Robot to run with bluetooth android app and wheel encoders for speed feedback

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Garr12100 0:10883d5ab141 1 #include "mbed.h"
Garr12100 0:10883d5ab141 2 #include "motordriver.h"
Garr12100 0:10883d5ab141 3
Garr12100 0:10883d5ab141 4 Serial pc(USBTX, USBRX);
Garr12100 0:10883d5ab141 5 Serial device(p9, p10); //Connect JY-MCU bluetooth module to 9,10
Garr12100 0:10883d5ab141 6
Garr12100 0:10883d5ab141 7 Motor right(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
Garr12100 0:10883d5ab141 8 Motor left(p26, p25, p24, 1);
Garr12100 0:10883d5ab141 9
Garr12100 0:10883d5ab141 10 DigitalIn Enc1(p19); //right motor
Garr12100 0:10883d5ab141 11 DigitalIn Enc2(p20); //left motor
Garr12100 0:10883d5ab141 12 int oldEnc1 = 0; //Was the encoder previously a 1 or zero?
Garr12100 0:10883d5ab141 13 int oldEnc2 = 0;
Garr12100 0:10883d5ab141 14
Garr12100 0:10883d5ab141 15 int ticksR = 0; //How many times has the Right wheel encoder changed state?
Garr12100 0:10883d5ab141 16 int ticksL = 0; //Same for left wheel
Garr12100 0:10883d5ab141 17 int E; // the error between the 2 wheel speeds
Garr12100 0:10883d5ab141 18 float speedL =0; //PWM speed setting for left wheel (scaled between 0 and 1)
Garr12100 0:10883d5ab141 19 float speedR=0; //Same for right wheel
Garr12100 0:10883d5ab141 20 Ticker Sampler; //Interrupt Routine to sample encoder ticks.
Garr12100 0:10883d5ab141 21 unsigned char Instr = 0;//What button was sent from app?
Garr12100 0:10883d5ab141 22 unsigned char sentSpeed = 0; //What is the speed slider set to?
Garr12100 0:10883d5ab141 23
Garr12100 0:10883d5ab141 24 void sampleEncoder(); //Function for sampling and adjusting speeds.
Garr12100 0:10883d5ab141 25
Garr12100 0:10883d5ab141 26 int main()
Garr12100 0:10883d5ab141 27 {
Garr12100 0:10883d5ab141 28 Sampler.attach(&sampleEncoder, .02); //Sampler uses sampleEncoder function every 20ms
Garr12100 0:10883d5ab141 29 Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
Garr12100 0:10883d5ab141 30 Enc2.mode(PullUp);
Garr12100 0:10883d5ab141 31 device.baud(57600); //Baud rate of JY-MCU (not Default!)
Garr12100 0:10883d5ab141 32
Garr12100 0:10883d5ab141 33 while(1) {
Garr12100 0:10883d5ab141 34 if(device.readable()) { //If the bluetooth has sent data, get the instruction
Garr12100 0:10883d5ab141 35 Instr = device.getc();
Garr12100 0:10883d5ab141 36 wait(.05);//Wait for the next bluetooth value to be recieved.
Garr12100 0:10883d5ab141 37 if(device.readable()) {//Now get the speed
Garr12100 0:10883d5ab141 38 sentSpeed = device.getc();
Garr12100 0:10883d5ab141 39 switch (Instr) {
Garr12100 0:10883d5ab141 40 case 0://Stop
Garr12100 0:10883d5ab141 41 left.speed(0);
Garr12100 0:10883d5ab141 42 right.speed(0);
Garr12100 0:10883d5ab141 43 break;
Garr12100 0:10883d5ab141 44 case 1://Forward
Garr12100 0:10883d5ab141 45
Garr12100 0:10883d5ab141 46 speedL = float(sentSpeed) / 100;;
Garr12100 0:10883d5ab141 47 speedR = speedL;
Garr12100 0:10883d5ab141 48 left.speed(speedL);
Garr12100 0:10883d5ab141 49 right.speed(speedR);
Garr12100 0:10883d5ab141 50 break;
Garr12100 0:10883d5ab141 51 case 2://Reverse (not currently implemented in app
Garr12100 0:10883d5ab141 52 speedL = -float(sentSpeed) / 100;;
Garr12100 0:10883d5ab141 53 speedR = speedL;
Garr12100 0:10883d5ab141 54 left.speed(speedL);
Garr12100 0:10883d5ab141 55 right.speed(speedR);
Garr12100 0:10883d5ab141 56 break;
Garr12100 0:10883d5ab141 57 case 3://Clockwise Turn
Garr12100 0:10883d5ab141 58 speedL = float(sentSpeed) / 100;
Garr12100 0:10883d5ab141 59 left.speed(speedL);
Garr12100 0:10883d5ab141 60 right.speed(-speedL);
Garr12100 0:10883d5ab141 61 break;
Garr12100 0:10883d5ab141 62 case 4://CCW Turn
Garr12100 0:10883d5ab141 63 speedL = float(sentSpeed) / 100;
Garr12100 0:10883d5ab141 64 left.speed(-speedL);
Garr12100 0:10883d5ab141 65 right.speed(speedL);
Garr12100 0:10883d5ab141 66 break;
Garr12100 0:10883d5ab141 67 }//End Switch
Garr12100 0:10883d5ab141 68
Garr12100 0:10883d5ab141 69 }//End Speed Read
Garr12100 0:10883d5ab141 70 }//End Instruction Read
Garr12100 0:10883d5ab141 71 if(Instr == 1 || Instr == 2) { // Only increment tick values if moving forward or reverse
Garr12100 0:10883d5ab141 72 if(Enc1 != oldEnc1) { // Increment ticks every time the state has switched.
Garr12100 0:10883d5ab141 73 ticksR++;
Garr12100 0:10883d5ab141 74 oldEnc1 = Enc1;
Garr12100 0:10883d5ab141 75
Garr12100 0:10883d5ab141 76 }
Garr12100 0:10883d5ab141 77 if(Enc2 != oldEnc2) {
Garr12100 0:10883d5ab141 78 ticksL++;
Garr12100 0:10883d5ab141 79 oldEnc2 = Enc2;
Garr12100 0:10883d5ab141 80 }
Garr12100 0:10883d5ab141 81 }
Garr12100 0:10883d5ab141 82
Garr12100 0:10883d5ab141 83 }
Garr12100 0:10883d5ab141 84 }
Garr12100 0:10883d5ab141 85
Garr12100 0:10883d5ab141 86 //Sample encoders and find error on right wheel. Assume Left wheel is always correct speed.
Garr12100 0:10883d5ab141 87 void sampleEncoder()
Garr12100 0:10883d5ab141 88 {
Garr12100 0:10883d5ab141 89 if((Instr == 1 || Instr == 2) && ticksL != 0) { //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped.
Garr12100 0:10883d5ab141 90
Garr12100 0:10883d5ab141 91 E = ticksL - ticksR; //Find error
Garr12100 0:10883d5ab141 92 speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
Garr12100 0:10883d5ab141 93 if(Instr == 1) speedR = speedR + float(E) / 255.0f;
Garr12100 0:10883d5ab141 94 //Reverse not implemented.
Garr12100 0:10883d5ab141 95 right.speed(speedR);
Garr12100 0:10883d5ab141 96 }
Garr12100 0:10883d5ab141 97 ticksR = 0; //Restart the counters
Garr12100 0:10883d5ab141 98 ticksL = 0;
Garr12100 0:10883d5ab141 99 }