Erik Fagan / Mbed 2 deprecated Robot_Bluetooth_remote_with_Hall_effect

Sparkfun Robot with Hall effect sensor and Bluetooth

Dependencies:   Motordriver mbed

main.cpp@0:e36c23af3037, 2014-10-18 (annotated)

Committer:
hydrogencellman
Date:
Sat Oct 18 18:06:15 2014 +0000
Revision:
0:e36c23af3037
Bluetooth remote and Hall-Effect encoder for feedback

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hydrogencellman 0:e36c23af3037 1 #include "mbed.h"
hydrogencellman 0:e36c23af3037 2 #include "motordriver.h"
hydrogencellman 0:e36c23af3037 3
hydrogencellman 0:e36c23af3037 4 Serial pc(USBTX, USBRX);
hydrogencellman 0:e36c23af3037 5 Serial device(p9, p10); //Connect JY-MCU bluetooth module to 9,10
hydrogencellman 0:e36c23af3037 6
hydrogencellman 0:e36c23af3037 7 Motor right(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
hydrogencellman 0:e36c23af3037 8 Motor left(p26, p25, p24, 1);
hydrogencellman 0:e36c23af3037 9
hydrogencellman 0:e36c23af3037 10 DigitalOut myled1(LED1);
hydrogencellman 0:e36c23af3037 11 DigitalOut myled2(LED2);
hydrogencellman 0:e36c23af3037 12 // LED's blink every pole change degugging like feature
hydrogencellman 0:e36c23af3037 13 DigitalOut myled3(LED3);
hydrogencellman 0:e36c23af3037 14 DigitalOut myled4(LED4);
hydrogencellman 0:e36c23af3037 15 void align();
hydrogencellman 0:e36c23af3037 16
hydrogencellman 0:e36c23af3037 17 // white wire is output, black is gound, Red is Vout or 3.3 volt
hydrogencellman 0:e36c23af3037 18 DigitalIn Enc1(p19); //right motor
hydrogencellman 0:e36c23af3037 19 DigitalIn Enc2(p20); //left motor
hydrogencellman 0:e36c23af3037 20 float speed;
hydrogencellman 0:e36c23af3037 21 //regulate speeds of motors
hydrogencellman 0:e36c23af3037 22 float s1 = 0;
hydrogencellman 0:e36c23af3037 23 float s2 = 0;
hydrogencellman 0:e36c23af3037 24 // this increases speed of motors slightly
hydrogencellman 0:e36c23af3037 25 int upspeed1 = 0;
hydrogencellman 0:e36c23af3037 26 int upspeed2 = 0;
hydrogencellman 0:e36c23af3037 27 // this is for correction that occurs for flag1 right motor speeding up
hydrogencellman 0:e36c23af3037 28 // or flage 2 for left motor.
hydrogencellman 0:e36c23af3037 29 int flag1 = 0;
hydrogencellman 0:e36c23af3037 30 int flag2 = 0;
hydrogencellman 0:e36c23af3037 31
hydrogencellman 0:e36c23af3037 32 int hexPos1 = 1; //4 north and 4 south poles -number 1-8
hydrogencellman 0:e36c23af3037 33 int hexPos2 = 1; // calling them postions on wheel.
hydrogencellman 0:e36c23af3037 34 // direction 1 = foward & dir = -1 is reverse
hydrogencellman 0:e36c23af3037 35 float dir = 1; //default set to forward
hydrogencellman 0:e36c23af3037 36 int state = 0;
hydrogencellman 0:e36c23af3037 37
hydrogencellman 0:e36c23af3037 38
hydrogencellman 0:e36c23af3037 39 int main()
hydrogencellman 0:e36c23af3037 40 {
hydrogencellman 0:e36c23af3037 41 unsigned char Instr = 0;//What button was sent? 1 = forward, 2 = backward
hydrogencellman 0:e36c23af3037 42 unsigned char sentSpeed; //What is the slider set to?
hydrogencellman 0:e36c23af3037 43 device.baud(57600);
hydrogencellman 0:e36c23af3037 44 Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
hydrogencellman 0:e36c23af3037 45 Enc2.mode(PullUp);
hydrogencellman 0:e36c23af3037 46 // this tells us the starting N/S poles of the motors
hydrogencellman 0:e36c23af3037 47
hydrogencellman 0:e36c23af3037 48 myled1 = (!Enc1);
hydrogencellman 0:e36c23af3037 49 myled2 = (!Enc2);
hydrogencellman 0:e36c23af3037 50 //intialize motors to start on same pole if not move motor two to correct
hydrogencellman 0:e36c23af3037 51 align();
hydrogencellman 0:e36c23af3037 52
hydrogencellman 0:e36c23af3037 53 while(1) {
hydrogencellman 0:e36c23af3037 54
hydrogencellman 0:e36c23af3037 55
hydrogencellman 0:e36c23af3037 56 if(device.readable()) {
hydrogencellman 0:e36c23af3037 57 Instr = device.getc();
hydrogencellman 0:e36c23af3037 58 pc.printf("Instruction: %i\n\r", Instr);
hydrogencellman 0:e36c23af3037 59 wait(.05);//Wait for the next bluetooth value to be recieved.
hydrogencellman 0:e36c23af3037 60 if(device.readable()) {
hydrogencellman 0:e36c23af3037 61 sentSpeed = device.getc();
hydrogencellman 0:e36c23af3037 62 switch (Instr) {
hydrogencellman 0:e36c23af3037 63 case 0://Stop
hydrogencellman 0:e36c23af3037 64 left.speed(0);
hydrogencellman 0:e36c23af3037 65 right.speed(0);
hydrogencellman 0:e36c23af3037 66 align();
hydrogencellman 0:e36c23af3037 67 break;
hydrogencellman 0:e36c23af3037 68 case 1://Forward
hydrogencellman 0:e36c23af3037 69 pc.printf("Speed: %i\n\r", sentSpeed);
hydrogencellman 0:e36c23af3037 70 speed = float(sentSpeed) / 100;
hydrogencellman 0:e36c23af3037 71 left.speed(speed);
hydrogencellman 0:e36c23af3037 72 right.speed(speed);
hydrogencellman 0:e36c23af3037 73 s1 = speed;
hydrogencellman 0:e36c23af3037 74 s2 = speed;
hydrogencellman 0:e36c23af3037 75 dir = 1;
hydrogencellman 0:e36c23af3037 76 break;
hydrogencellman 0:e36c23af3037 77 case 2://Reverse
hydrogencellman 0:e36c23af3037 78 pc.printf("Reverse speed: %i\n\r", sentSpeed);
hydrogencellman 0:e36c23af3037 79 hexPos1 = 0;
hydrogencellman 0:e36c23af3037 80 hexPos2 = 0;
hydrogencellman 0:e36c23af3037 81 speed = -float(sentSpeed) / 100;
hydrogencellman 0:e36c23af3037 82 left.speed(speed);
hydrogencellman 0:e36c23af3037 83 right.speed(speed);
hydrogencellman 0:e36c23af3037 84 s1 = speed;
hydrogencellman 0:e36c23af3037 85 s2 = speed;
hydrogencellman 0:e36c23af3037 86 dir = -1;
hydrogencellman 0:e36c23af3037 87 break;
hydrogencellman 0:e36c23af3037 88 case 3://Clockwise Turn
hydrogencellman 0:e36c23af3037 89 speed = float(sentSpeed) / 100;
hydrogencellman 0:e36c23af3037 90 left.speed(speed);
hydrogencellman 0:e36c23af3037 91 right.speed(-speed);
hydrogencellman 0:e36c23af3037 92 break;
hydrogencellman 0:e36c23af3037 93 case 4://CCW Turn
hydrogencellman 0:e36c23af3037 94 speed = float(sentSpeed) / 100;
hydrogencellman 0:e36c23af3037 95 left.speed(-speed);
hydrogencellman 0:e36c23af3037 96 right.speed(speed);
hydrogencellman 0:e36c23af3037 97 break;
hydrogencellman 0:e36c23af3037 98 }//End Switch
hydrogencellman 0:e36c23af3037 99
hydrogencellman 0:e36c23af3037 100 }//End Speed Read
hydrogencellman 0:e36c23af3037 101 }//End Instruction Read
hydrogencellman 0:e36c23af3037 102
hydrogencellman 0:e36c23af3037 103
hydrogencellman 0:e36c23af3037 104 // forward and reverse Instructions from app are 2 and 1
hydrogencellman 0:e36c23af3037 105 if((Instr == 2) || (Instr == 1)) {
hydrogencellman 0:e36c23af3037 106 // Right motor faster then left
hydrogencellman 0:e36c23af3037 107 if(hexPos1 >= hexPos2) {
hydrogencellman 0:e36c23af3037 108 // this takes out negative values as being < 2
hydrogencellman 0:e36c23af3037 109 if(((hexPos1 - hexPos2) < 2) ) {
hydrogencellman 0:e36c23af3037 110
hydrogencellman 0:e36c23af3037 111 if(flag2) {
hydrogencellman 0:e36c23af3037 112 // now that left side is caught up reduce speed increased by half since
hydrogencellman 0:e36c23af3037 113 // it was slower to bein with.
hydrogencellman 0:e36c23af3037 114 left.speed(s2 -= (upspeed2*0.01*dir));
hydrogencellman 0:e36c23af3037 115 flag2 = 0;
hydrogencellman 0:e36c23af3037 116 }
hydrogencellman 0:e36c23af3037 117
hydrogencellman 0:e36c23af3037 118
hydrogencellman 0:e36c23af3037 119 }
hydrogencellman 0:e36c23af3037 120 if(((hexPos1 - hexPos2) > 2) ) {
hydrogencellman 0:e36c23af3037 121 // requires corrective action
hydrogencellman 0:e36c23af3037 122 flag2 = 1;
hydrogencellman 0:e36c23af3037 123 if(dir*s2<1) {
hydrogencellman 0:e36c23af3037 124 left.speed(s2 +=(dir*0.02));
hydrogencellman 0:e36c23af3037 125 upspeed2 += 1;
hydrogencellman 0:e36c23af3037 126 }
hydrogencellman 0:e36c23af3037 127
hydrogencellman 0:e36c23af3037 128 }
hydrogencellman 0:e36c23af3037 129 }
hydrogencellman 0:e36c23af3037 130
hydrogencellman 0:e36c23af3037 131
hydrogencellman 0:e36c23af3037 132 // Left motor faster then right
hydrogencellman 0:e36c23af3037 133 if(hexPos2 > hexPos1) {
hydrogencellman 0:e36c23af3037 134 // this takes out negative values as being < 2
hydrogencellman 0:e36c23af3037 135 if(((hexPos2 - hexPos1) < 2)) {
hydrogencellman 0:e36c23af3037 136
hydrogencellman 0:e36c23af3037 137 if(flag1) {
hydrogencellman 0:e36c23af3037 138 // now that left side is caught up reduce speed increased by half since
hydrogencellman 0:e36c23af3037 139 // it was slower to bein with.
hydrogencellman 0:e36c23af3037 140 right.speed(s1 -= (upspeed1*0.01*dir));
hydrogencellman 0:e36c23af3037 141 flag1 = 0;
hydrogencellman 0:e36c23af3037 142 }
hydrogencellman 0:e36c23af3037 143
hydrogencellman 0:e36c23af3037 144 }
hydrogencellman 0:e36c23af3037 145 if(((hexPos2 - hexPos1) > 2) ) {
hydrogencellman 0:e36c23af3037 146 // requires corrective action
hydrogencellman 0:e36c23af3037 147 flag1 = 1;
hydrogencellman 0:e36c23af3037 148 if(dir*s1<1) {
hydrogencellman 0:e36c23af3037 149 right.speed(s1 +=(0.02*dir));
hydrogencellman 0:e36c23af3037 150 upspeed1 += 1;
hydrogencellman 0:e36c23af3037 151 }
hydrogencellman 0:e36c23af3037 152
hydrogencellman 0:e36c23af3037 153 }
hydrogencellman 0:e36c23af3037 154 }
hydrogencellman 0:e36c23af3037 155 // keeps count low.
hydrogencellman 0:e36c23af3037 156 if(hexPos1 > 2000 || hexPos2 > 2000) {
hydrogencellman 0:e36c23af3037 157 hexPos1 = hexPos1-1000;
hydrogencellman 0:e36c23af3037 158 hexPos2 = hexPos2-1000;
hydrogencellman 0:e36c23af3037 159 }
hydrogencellman 0:e36c23af3037 160
hydrogencellman 0:e36c23af3037 161
hydrogencellman 0:e36c23af3037 162 // this is for keeping track of the poles and updating LED's so you can see
hydrogencellman 0:e36c23af3037 163 // if the motors are getting out of wack.
hydrogencellman 0:e36c23af3037 164 if(!Enc1) { // I'm assuming this is a postive pole
hydrogencellman 0:e36c23af3037 165 if(!myled1) { // it was a south pole last time and is now north
hydrogencellman 0:e36c23af3037 166 hexPos1++;
hydrogencellman 0:e36c23af3037 167 myled3 = !myled3;
hydrogencellman 0:e36c23af3037 168 myled1 = 1;
hydrogencellman 0:e36c23af3037 169 }
hydrogencellman 0:e36c23af3037 170 } else {
hydrogencellman 0:e36c23af3037 171 if(myled1) { // it was a North pole last time and is now South
hydrogencellman 0:e36c23af3037 172 hexPos1++;
hydrogencellman 0:e36c23af3037 173 myled3 = !myled3;
hydrogencellman 0:e36c23af3037 174 myled1 = 0;
hydrogencellman 0:e36c23af3037 175 }
hydrogencellman 0:e36c23af3037 176 }
hydrogencellman 0:e36c23af3037 177
hydrogencellman 0:e36c23af3037 178 if(!Enc2) { // I'm assuming this is a postive pole
hydrogencellman 0:e36c23af3037 179 if(!myled2) { // it was a south pole last time and is now north
hydrogencellman 0:e36c23af3037 180 hexPos2++;
hydrogencellman 0:e36c23af3037 181 myled4= !myled4;
hydrogencellman 0:e36c23af3037 182 myled2 = 1;
hydrogencellman 0:e36c23af3037 183 }
hydrogencellman 0:e36c23af3037 184 } else {
hydrogencellman 0:e36c23af3037 185 if(myled2) { // it was a North pole last time and is now South
hydrogencellman 0:e36c23af3037 186 hexPos2++;
hydrogencellman 0:e36c23af3037 187 myled4 = !myled4;
hydrogencellman 0:e36c23af3037 188 myled2 = 0;
hydrogencellman 0:e36c23af3037 189 }
hydrogencellman 0:e36c23af3037 190 }
hydrogencellman 0:e36c23af3037 191 }
hydrogencellman 0:e36c23af3037 192
hydrogencellman 0:e36c23af3037 193 }
hydrogencellman 0:e36c23af3037 194 }
hydrogencellman 0:e36c23af3037 195
hydrogencellman 0:e36c23af3037 196
hydrogencellman 0:e36c23af3037 197 //intialize motors to start on same pole if not move motor two to correct
hydrogencellman 0:e36c23af3037 198 void align()
hydrogencellman 0:e36c23af3037 199 {
hydrogencellman 0:e36c23af3037 200 if(Enc1 != Enc2) {
hydrogencellman 0:e36c23af3037 201 while(1) {
hydrogencellman 0:e36c23af3037 202 left.speed(.5);
hydrogencellman 0:e36c23af3037 203 if(Enc1 == Enc2) {
hydrogencellman 0:e36c23af3037 204 left.speed(0);
hydrogencellman 0:e36c23af3037 205 myled1 = (!Enc1);
hydrogencellman 0:e36c23af3037 206 myled2 = (!Enc2);
hydrogencellman 0:e36c23af3037 207 hexPos1 = 0;
hydrogencellman 0:e36c23af3037 208 hexPos2 = 0;
hydrogencellman 0:e36c23af3037 209 break;
hydrogencellman 0:e36c23af3037 210 }
hydrogencellman 0:e36c23af3037 211 }
hydrogencellman 0:e36c23af3037 212 }
hydrogencellman 0:e36c23af3037 213 }