Garrett Leach
Allows Magician Robot to run with Android Bluetooth app and wheel encoders to make wheel turn at the same time.
Dependencies: Motordriver mbed
Revision 0:10883d5ab141, committed 2014-10-19
- Comitter:
- Garr12100
- Date:
- Sun Oct 19 02:29:13 2014 +0000
- Commit message:
- Allows Magician Robot to run with bluetooth android app and wheel encoders for speed feedback
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motordriver.lib Sun Oct 19 02:29:13 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/littlexc/code/Motordriver/#3110b9209d3c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Oct 19 02:29:13 2014 +0000
@@ -0,0 +1,99 @@
+#include "mbed.h"
+#include "motordriver.h"
+
+Serial pc(USBTX, USBRX);
+Serial device(p9, p10); //Connect JY-MCU bluetooth module to 9,10
+
+Motor right(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
+Motor left(p26, p25, p24, 1);
+
+DigitalIn Enc1(p19); //right motor
+DigitalIn Enc2(p20); //left motor
+int oldEnc1 = 0; //Was the encoder previously a 1 or zero?
+int oldEnc2 = 0;
+
+int ticksR = 0; //How many times has the Right wheel encoder changed state?
+int ticksL = 0; //Same for left wheel
+int E; // the error between the 2 wheel speeds
+float speedL =0; //PWM speed setting for left wheel (scaled between 0 and 1)
+float speedR=0; //Same for right wheel
+Ticker Sampler; //Interrupt Routine to sample encoder ticks.
+unsigned char Instr = 0;//What button was sent from app?
+unsigned char sentSpeed = 0; //What is the speed slider set to?
+
+void sampleEncoder(); //Function for sampling and adjusting speeds.
+
+int main()
+{
+ Sampler.attach(&sampleEncoder, .02); //Sampler uses sampleEncoder function every 20ms
+ Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
+ Enc2.mode(PullUp);
+ device.baud(57600); //Baud rate of JY-MCU (not Default!)
+
+ while(1) {
+ if(device.readable()) { //If the bluetooth has sent data, get the instruction
+ Instr = device.getc();
+ wait(.05);//Wait for the next bluetooth value to be recieved.
+ if(device.readable()) {//Now get the speed
+ sentSpeed = device.getc();
+ switch (Instr) {
+ case 0://Stop
+ left.speed(0);
+ right.speed(0);
+ break;
+ case 1://Forward
+
+ speedL = float(sentSpeed) / 100;;
+ speedR = speedL;
+ left.speed(speedL);
+ right.speed(speedR);
+ break;
+ case 2://Reverse (not currently implemented in app
+ speedL = -float(sentSpeed) / 100;;
+ speedR = speedL;
+ left.speed(speedL);
+ right.speed(speedR);
+ break;
+ case 3://Clockwise Turn
+ speedL = float(sentSpeed) / 100;
+ left.speed(speedL);
+ right.speed(-speedL);
+ break;
+ case 4://CCW Turn
+ speedL = float(sentSpeed) / 100;
+ left.speed(-speedL);
+ right.speed(speedL);
+ break;
+ }//End Switch
+
+ }//End Speed Read
+ }//End Instruction Read
+ if(Instr == 1 || Instr == 2) { // Only increment tick values if moving forward or reverse
+ if(Enc1 != oldEnc1) { // Increment ticks every time the state has switched.
+ ticksR++;
+ oldEnc1 = Enc1;
+
+ }
+ if(Enc2 != oldEnc2) {
+ ticksL++;
+ oldEnc2 = Enc2;
+ }
+ }
+
+ }
+}
+
+//Sample encoders and find error on right wheel. Assume Left wheel is always correct speed.
+void sampleEncoder()
+{
+ if((Instr == 1 || Instr == 2) && ticksL != 0) { //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped.
+
+ E = ticksL - ticksR; //Find error
+ speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
+ if(Instr == 1) speedR = speedR + float(E) / 255.0f;
+ //Reverse not implemented.
+ right.speed(speedR);
+ }
+ ticksR = 0; //Restart the counters
+ ticksL = 0;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Oct 19 02:29:13 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/552587b429a1 \ No newline at end of file