Erik Fagan
Sparkfun Robot with Hall effect sensor and Bluetooth
Dependencies: Motordriver mbed
Revision 0:e36c23af3037, committed 2014-10-18
- Comitter:
- hydrogencellman
- Date:
- Sat Oct 18 18:06:15 2014 +0000
- Commit message:
- Bluetooth remote and Hall-Effect encoder for feedback
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motordriver.lib Sat Oct 18 18:06:15 2014 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/littlexc/code/Motordriver/#3110b9209d3c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Oct 18 18:06:15 2014 +0000
@@ -0,0 +1,213 @@
+#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);
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+// LED's blink every pole change degugging like feature
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+void align();
+
+// white wire is output, black is gound, Red is Vout or 3.3 volt
+DigitalIn Enc1(p19); //right motor
+DigitalIn Enc2(p20); //left motor
+float speed;
+//regulate speeds of motors
+float s1 = 0;
+float s2 = 0;
+// this increases speed of motors slightly
+int upspeed1 = 0;
+int upspeed2 = 0;
+// this is for correction that occurs for flag1 right motor speeding up
+// or flage 2 for left motor.
+int flag1 = 0;
+int flag2 = 0;
+
+int hexPos1 = 1; //4 north and 4 south poles -number 1-8
+int hexPos2 = 1; // calling them postions on wheel.
+// direction 1 = foward & dir = -1 is reverse
+float dir = 1; //default set to forward
+int state = 0;
+
+
+int main()
+{
+ unsigned char Instr = 0;//What button was sent? 1 = forward, 2 = backward
+ unsigned char sentSpeed; //What is the slider set to?
+ device.baud(57600);
+ Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
+ Enc2.mode(PullUp);
+ // this tells us the starting N/S poles of the motors
+
+ myled1 = (!Enc1);
+ myled2 = (!Enc2);
+ //intialize motors to start on same pole if not move motor two to correct
+ align();
+
+ while(1) {
+
+
+ if(device.readable()) {
+ Instr = device.getc();
+ pc.printf("Instruction: %i\n\r", Instr);
+ wait(.05);//Wait for the next bluetooth value to be recieved.
+ if(device.readable()) {
+ sentSpeed = device.getc();
+ switch (Instr) {
+ case 0://Stop
+ left.speed(0);
+ right.speed(0);
+ align();
+ break;
+ case 1://Forward
+ pc.printf("Speed: %i\n\r", sentSpeed);
+ speed = float(sentSpeed) / 100;
+ left.speed(speed);
+ right.speed(speed);
+ s1 = speed;
+ s2 = speed;
+ dir = 1;
+ break;
+ case 2://Reverse
+ pc.printf("Reverse speed: %i\n\r", sentSpeed);
+ hexPos1 = 0;
+ hexPos2 = 0;
+ speed = -float(sentSpeed) / 100;
+ left.speed(speed);
+ right.speed(speed);
+ s1 = speed;
+ s2 = speed;
+ dir = -1;
+ break;
+ case 3://Clockwise Turn
+ speed = float(sentSpeed) / 100;
+ left.speed(speed);
+ right.speed(-speed);
+ break;
+ case 4://CCW Turn
+ speed = float(sentSpeed) / 100;
+ left.speed(-speed);
+ right.speed(speed);
+ break;
+ }//End Switch
+
+ }//End Speed Read
+ }//End Instruction Read
+
+
+ // forward and reverse Instructions from app are 2 and 1
+ if((Instr == 2) || (Instr == 1)) {
+ // Right motor faster then left
+ if(hexPos1 >= hexPos2) {
+ // this takes out negative values as being < 2
+ if(((hexPos1 - hexPos2) < 2) ) {
+
+ if(flag2) {
+ // now that left side is caught up reduce speed increased by half since
+ // it was slower to bein with.
+ left.speed(s2 -= (upspeed2*0.01*dir));
+ flag2 = 0;
+ }
+
+
+ }
+ if(((hexPos1 - hexPos2) > 2) ) {
+ // requires corrective action
+ flag2 = 1;
+ if(dir*s2<1) {
+ left.speed(s2 +=(dir*0.02));
+ upspeed2 += 1;
+ }
+
+ }
+ }
+
+
+ // Left motor faster then right
+ if(hexPos2 > hexPos1) {
+ // this takes out negative values as being < 2
+ if(((hexPos2 - hexPos1) < 2)) {
+
+ if(flag1) {
+ // now that left side is caught up reduce speed increased by half since
+ // it was slower to bein with.
+ right.speed(s1 -= (upspeed1*0.01*dir));
+ flag1 = 0;
+ }
+
+ }
+ if(((hexPos2 - hexPos1) > 2) ) {
+ // requires corrective action
+ flag1 = 1;
+ if(dir*s1<1) {
+ right.speed(s1 +=(0.02*dir));
+ upspeed1 += 1;
+ }
+
+ }
+ }
+ // keeps count low.
+ if(hexPos1 > 2000 || hexPos2 > 2000) {
+ hexPos1 = hexPos1-1000;
+ hexPos2 = hexPos2-1000;
+ }
+
+
+ // this is for keeping track of the poles and updating LED's so you can see
+ // if the motors are getting out of wack.
+ if(!Enc1) { // I'm assuming this is a postive pole
+ if(!myled1) { // it was a south pole last time and is now north
+ hexPos1++;
+ myled3 = !myled3;
+ myled1 = 1;
+ }
+ } else {
+ if(myled1) { // it was a North pole last time and is now South
+ hexPos1++;
+ myled3 = !myled3;
+ myled1 = 0;
+ }
+ }
+
+ if(!Enc2) { // I'm assuming this is a postive pole
+ if(!myled2) { // it was a south pole last time and is now north
+ hexPos2++;
+ myled4= !myled4;
+ myled2 = 1;
+ }
+ } else {
+ if(myled2) { // it was a North pole last time and is now South
+ hexPos2++;
+ myled4 = !myled4;
+ myled2 = 0;
+ }
+ }
+ }
+
+ }
+}
+
+
+//intialize motors to start on same pole if not move motor two to correct
+void align()
+{
+ if(Enc1 != Enc2) {
+ while(1) {
+ left.speed(.5);
+ if(Enc1 == Enc2) {
+ left.speed(0);
+ myled1 = (!Enc1);
+ myled2 = (!Enc2);
+ hexPos1 = 0;
+ hexPos2 = 0;
+ break;
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Oct 18 18:06:15 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/552587b429a1 \ No newline at end of file