Josh Schrader / Mbed 2 deprecated EtchSketchSmartbot

Semi-Functional EtchSketch Smartbot code. Takes x/y values and draws with polar coords.

main.cpp@0:eae9712515a6, 2012-12-15 (annotated)

Committer:
theschrade54
Date:
Sat Dec 15 01:48:19 2012 +0000
Revision:
0:eae9712515a6
Semi-Functional EtchSketch Smartbot code. Takes x/y values and draws with polar coords.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
theschrade54 0:eae9712515a6 1 #include "mbed.h"
theschrade54 0:eae9712515a6 2 #include "motordriver.h"
theschrade54 0:eae9712515a6 3 #include "Servo.h"
theschrade54 0:eae9712515a6 4 #include "HMC6352.h"
theschrade54 0:eae9712515a6 5
theschrade54 0:eae9712515a6 6 HMC6352 compass(p9, p10);
theschrade54 0:eae9712515a6 7 //Smartbot
theschrade54 0:eae9712515a6 8
theschrade54 0:eae9712515a6 9 DigitalOut mbedled1(LED1); //signals draw mode
theschrade54 0:eae9712515a6 10 DigitalOut mbedled2(LED2); //singals relocate mode
theschrade54 0:eae9712515a6 11 DigitalOut mbedled3(LED2); //signals left wheel movement
theschrade54 0:eae9712515a6 12 DigitalOut mbedled4(LED2); //signals right wheel movement
theschrade54 0:eae9712515a6 13
theschrade54 0:eae9712515a6 14 Serial pc(USBTX, USBRX); // tx, rx
theschrade54 0:eae9712515a6 15 Serial controller(p13, p14); //change to whatever rcv method we use
theschrade54 0:eae9712515a6 16
theschrade54 0:eae9712515a6 17 //See http://mbed.org/cookbook/Motor
theschrade54 0:eae9712515a6 18 //http://mbed.org/users/4180_1/notebook/sparkfun-magician-robot-base-kit/
theschrade54 0:eae9712515a6 19 //Connections to dual H-brdige driver for the two drive motors
theschrade54 0:eae9712515a6 20 Motor left(p21, p22, p20, 1); // pwm, fwd, rev, has brake feature
theschrade54 0:eae9712515a6 21 Motor right(p24, p25, p27, 1);
theschrade54 0:eae9712515a6 22
theschrade54 0:eae9712515a6 23 Servo penservo(p23);
theschrade54 0:eae9712515a6 24
theschrade54 0:eae9712515a6 25 int main()
theschrade54 0:eae9712515a6 26 {
theschrade54 0:eae9712515a6 27 //init command vars
theschrade54 0:eae9712515a6 28 char command = 0; //bit0 = 1 if shake; bit7 = 0 if y, 1 if x; bit6 = 0 if pos, 1 if neg
theschrade54 0:eae9712515a6 29 char XorY = 128; // X or Y move command bitmask
theschrade54 0:eae9712515a6 30 char NorP = 64; // Down or Up move command bitmask
theschrade54 0:eae9712515a6 31 char mag = 62; // bitmask for magnitude of move command
theschrade54 0:eae9712515a6 32 char magnitude = 0; //magnitude of move
theschrade54 0:eae9712515a6 33
theschrade54 0:eae9712515a6 34 //Direction and Movement Variables, start on page 0 facing +y with dx, dy =0
theschrade54 0:eae9712515a6 35 float nphi, myphi, dphi = 0.0; //angle math, (36)0 = +y, 90 = +x, 180 = -y, 270 = -x
theschrade54 0:eae9712515a6 36 float dy, dx = 0.0; // where on page?
theschrade54 0:eae9712515a6 37 float ny, nx = 0.0; // next move command coords
theschrade54 0:eae9712515a6 38 //int pagesize = 1050; //square page side draw length + border
theschrade54 0:eae9712515a6 39 float moveweight = 0; //length of current move order, effects speed
theschrade54 0:eae9712515a6 40 int comcount = 4; //number of move commands to add into 1 move order
theschrade54 0:eae9712515a6 41 // int flip = 0; //backwards to save time?
theschrade54 0:eae9712515a6 42 int i;
theschrade54 0:eae9712515a6 43
theschrade54 0:eae9712515a6 44 //Init Servo
theschrade54 0:eae9712515a6 45 penservo = .5;
theschrade54 0:eae9712515a6 46
theschrade54 0:eae9712515a6 47 //Initialize LEDs
theschrade54 0:eae9712515a6 48 mbedled1 = 1;
theschrade54 0:eae9712515a6 49 mbedled2 = 0;
theschrade54 0:eae9712515a6 50
theschrade54 0:eae9712515a6 51 //Init IR Receiver
theschrade54 0:eae9712515a6 52 controller.baud(2400);
theschrade54 0:eae9712515a6 53 wait(1);
theschrade54 0:eae9712515a6 54
theschrade54 0:eae9712515a6 55 //Init Compass to continuous mode, periodic set/reset, 20Hz measurement rate.
theschrade54 0:eae9712515a6 56 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
theschrade54 0:eae9712515a6 57 wait(3);
theschrade54 0:eae9712515a6 58
theschrade54 0:eae9712515a6 59 //Init Compass reading to 0
theschrade54 0:eae9712515a6 60 for (i = 0; i < 5; i++) { //Take Averaged Compass reading
theschrade54 0:eae9712515a6 61 myphi += compass.sample()/50.0;
theschrade54 0:eae9712515a6 62 wait(.05);
theschrade54 0:eae9712515a6 63 }
theschrade54 0:eae9712515a6 64 dphi = nphi - myphi; // calc angle diff
theschrade54 0:eae9712515a6 65 while (abs(dphi) > 15) { // if diff sig, turn to 0
theschrade54 0:eae9712515a6 66 myphi = 0.0;
theschrade54 0:eae9712515a6 67 left.speed(-.5);
theschrade54 0:eae9712515a6 68 for (i = 0; i < 3; i++) { //Take Averaged Compass reading
theschrade54 0:eae9712515a6 69 myphi += compass.sample()/30.0;
theschrade54 0:eae9712515a6 70 wait(.01);
theschrade54 0:eae9712515a6 71 }
theschrade54 0:eae9712515a6 72 dphi = nphi - myphi;
theschrade54 0:eae9712515a6 73 wait(.01);
theschrade54 0:eae9712515a6 74 }
theschrade54 0:eae9712515a6 75 right.stop(1); //stop wheel(s) after turn
theschrade54 0:eae9712515a6 76 left.stop(1);
theschrade54 0:eae9712515a6 77 wait(.5);
theschrade54 0:eae9712515a6 78
theschrade54 0:eae9712515a6 79 //pc.printf("myphi: %f", myphi);
theschrade54 0:eae9712515a6 80 //pc.printf("starting \n");
theschrade54 0:eae9712515a6 81
theschrade54 0:eae9712515a6 82 while(1) {
theschrade54 0:eae9712515a6 83
theschrade54 0:eae9712515a6 84 //if readable, get command
theschrade54 0:eae9712515a6 85 if(controller.readable()) {
theschrade54 0:eae9712515a6 86 mbedled2 = 1;
theschrade54 0:eae9712515a6 87 command = controller.getc(); //Serial read
theschrade54 0:eae9712515a6 88 comcount--;
theschrade54 0:eae9712515a6 89 //pc.printf("%x \n", (command));
theschrade54 0:eae9712515a6 90 mbedled2 = 0;
theschrade54 0:eae9712515a6 91 } else if (comcount < 4) { //decrement if command countdown started even if no command read
theschrade54 0:eae9712515a6 92 command = 0;
theschrade54 0:eae9712515a6 93 comcount--;
theschrade54 0:eae9712515a6 94 } else {
theschrade54 0:eae9712515a6 95 command = 0;
theschrade54 0:eae9712515a6 96 }
theschrade54 0:eae9712515a6 97
theschrade54 0:eae9712515a6 98 //check for shake
theschrade54 0:eae9712515a6 99 if (command==1) { //lift pen up and reposition
theschrade54 0:eae9712515a6 100 mbedled1 = 0;
theschrade54 0:eae9712515a6 101 penservo = 1;
theschrade54 0:eae9712515a6 102 wait(3);
theschrade54 0:eae9712515a6 103 dx = 0; //reset vars after reposition
theschrade54 0:eae9712515a6 104 dy = 0;
theschrade54 0:eae9712515a6 105 comcount = 5;
theschrade54 0:eae9712515a6 106 mbedled1 = 1;
theschrade54 0:eae9712515a6 107 penservo = .5;
theschrade54 0:eae9712515a6 108 }
theschrade54 0:eae9712515a6 109
theschrade54 0:eae9712515a6 110 //check for move command
theschrade54 0:eae9712515a6 111 if ((abs(command) > 1) && (comcount > 0)) {
theschrade54 0:eae9712515a6 112 magnitude = mag & command;
theschrade54 0:eae9712515a6 113 if ( (command & XorY) == 128 ) { // if bit7 then X axis
theschrade54 0:eae9712515a6 114 if ( (command & NorP) == 64 ) { // if bit6 then -x
theschrade54 0:eae9712515a6 115 // nx -= (.3+magnitude/16.0)/5.0;
theschrade54 0:eae9712515a6 116 nx -= magnitude / 4.0;
theschrade54 0:eae9712515a6 117 } else { //else if !bit6 then +x
theschrade54 0:eae9712515a6 118 // nx += (.3+magnitude/16.0)/5.0;
theschrade54 0:eae9712515a6 119 nx += magnitude / 4.0;
theschrade54 0:eae9712515a6 120 }
theschrade54 0:eae9712515a6 121 } else { //else if !bit7 then Y axis
theschrade54 0:eae9712515a6 122 if ( (command & NorP) == 64 ) { // if bit6 then -y
theschrade54 0:eae9712515a6 123 // ny -= (.3+magnitude/16.0)/5.0;
theschrade54 0:eae9712515a6 124 ny -= magnitude / 4.0;
theschrade54 0:eae9712515a6 125 } else { //else if !bit6 then +y
theschrade54 0:eae9712515a6 126 // ny += (.3+magnitude/16.0)/5.0;
theschrade54 0:eae9712515a6 127 ny += magnitude / 4.0;
theschrade54 0:eae9712515a6 128 }
theschrade54 0:eae9712515a6 129 }
theschrade54 0:eae9712515a6 130 wait(.05);
theschrade54 0:eae9712515a6 131 } else if (comcount == 0) { //move math
theschrade54 0:eae9712515a6 132 if ((nx != 0) && (ny != 0)) { // if both x&y change, calculate vector angle and length in polar coords
theschrade54 0:eae9712515a6 133 dx += nx;
theschrade54 0:eae9712515a6 134 dy += ny;
theschrade54 0:eae9712515a6 135 moveweight = pow(ny, 2) + pow(nx, 2); //moveweight = r
theschrade54 0:eae9712515a6 136 moveweight = sqrt(moveweight);
theschrade54 0:eae9712515a6 137 nphi = atan2(nx, ny); //new phi = principal arctan from -180 to 180. x and y flipped to get angle from y to x
theschrade54 0:eae9712515a6 138 //pc.printf("POLAR myphi: %f, newphi: %f, movelength: %f \n", myphi, nphi, moveweight);
theschrade54 0:eae9712515a6 139 } else {
theschrade54 0:eae9712515a6 140 dphi = 0;
theschrade54 0:eae9712515a6 141 moveweight = 0;
theschrade54 0:eae9712515a6 142 }
theschrade54 0:eae9712515a6 143
theschrade54 0:eae9712515a6 144 for (i = 0; i < 3; i++) { //Take Averaged Compass reading
theschrade54 0:eae9712515a6 145 myphi += compass.sample()/30.0;
theschrade54 0:eae9712515a6 146 wait(.01);
theschrade54 0:eae9712515a6 147 }
theschrade54 0:eae9712515a6 148
theschrade54 0:eae9712515a6 149 if (abs(nx) < 2) { //if only signif. y axis move
theschrade54 0:eae9712515a6 150 moveweight = abs(ny); // set speed
theschrade54 0:eae9712515a6 151 if (ny < -2) {nphi = 180;} // set angles to axis
theschrade54 0:eae9712515a6 152 else if (ny > 2) {nphi = 0;}
theschrade54 0:eae9712515a6 153 else {nphi = myphi;
theschrade54 0:eae9712515a6 154 moveweight = 0;}
theschrade54 0:eae9712515a6 155 }
theschrade54 0:eae9712515a6 156
theschrade54 0:eae9712515a6 157 if (abs(ny) < 2) { //if only signif. x axis move
theschrade54 0:eae9712515a6 158 moveweight = abs(nx);
theschrade54 0:eae9712515a6 159 if (nx < -2) {nphi = 270;}
theschrade54 0:eae9712515a6 160 else if (nx > 2) {nphi = 90;}
theschrade54 0:eae9712515a6 161 else {nphi = myphi;
theschrade54 0:eae9712515a6 162 moveweight = 0;}
theschrade54 0:eae9712515a6 163 }
theschrade54 0:eae9712515a6 164
theschrade54 0:eae9712515a6 165 dphi = nphi - myphi; // calc angle diff
theschrade54 0:eae9712515a6 166 //if (abs(dphi) > 340) { dphi = 0;} // fix pos y axis
theschrade54 0:eae9712515a6 167 /*if ((160 < abs(dphi)) && (abs(dphi)< 200)) {
theschrade54 0:eae9712515a6 168 moveweight = -moveweight;
theschrade54 0:eae9712515a6 169 dphi = 0;} //flip speed instead of flip around
theschrade54 0:eae9712515a6 170 if (dphi > 180) { // switch angle diff with mag above 180 and flip
theschrade54 0:eae9712515a6 171 nphi -= 180;
theschrade54 0:eae9712515a6 172 moveweight = -moveweight; //flip speed
theschrade54 0:eae9712515a6 173 }
theschrade54 0:eae9712515a6 174 if (dphi < -180) {
theschrade54 0:eae9712515a6 175 nphi += 180;
theschrade54 0:eae9712515a6 176 moveweight = -moveweight; //flip speed
theschrade54 0:eae9712515a6 177 }
theschrade54 0:eae9712515a6 178 */
theschrade54 0:eae9712515a6 179
theschrade54 0:eae9712515a6 180
theschrade54 0:eae9712515a6 181 if (abs(dphi) > 15) { //turn left
theschrade54 0:eae9712515a6 182 right.stop(1); //stop wheels before turn
theschrade54 0:eae9712515a6 183 left.stop(1);
theschrade54 0:eae9712515a6 184 wait(.1);
theschrade54 0:eae9712515a6 185 while (abs(dphi) > 15) { // if diff sig, turn to 0
theschrade54 0:eae9712515a6 186 myphi = 0.0;
theschrade54 0:eae9712515a6 187 left.speed(-.5);
theschrade54 0:eae9712515a6 188 for (i = 0; i < 2; i++) { //Take quick Averaged Compass reading
theschrade54 0:eae9712515a6 189 myphi += compass.sample()/20.0;
theschrade54 0:eae9712515a6 190 wait(.01);}
theschrade54 0:eae9712515a6 191 dphi = nphi - myphi;
theschrade54 0:eae9712515a6 192 }
theschrade54 0:eae9712515a6 193 right.stop(1); //stop wheels after turn
theschrade54 0:eae9712515a6 194 left.stop(1);
theschrade54 0:eae9712515a6 195 wait(.1);
theschrade54 0:eae9712515a6 196 }
theschrade54 0:eae9712515a6 197
theschrade54 0:eae9712515a6 198 while (moveweight >= 1) { //go fwd for r
theschrade54 0:eae9712515a6 199 left.speed(.7);
theschrade54 0:eae9712515a6 200 right.speed(.7);
theschrade54 0:eae9712515a6 201 wait(.1);
theschrade54 0:eae9712515a6 202 moveweight--;
theschrade54 0:eae9712515a6 203 //moveweight = moveweight/2;
theschrade54 0:eae9712515a6 204 }
theschrade54 0:eae9712515a6 205
theschrade54 0:eae9712515a6 206 while (moveweight <= -1) { //go back for r
theschrade54 0:eae9712515a6 207 left.speed(-.7);
theschrade54 0:eae9712515a6 208 right.speed(-.7);
theschrade54 0:eae9712515a6 209 wait(.1);
theschrade54 0:eae9712515a6 210 moveweight++;
theschrade54 0:eae9712515a6 211 //moveweight = moveweight/2;
theschrade54 0:eae9712515a6 212 }
theschrade54 0:eae9712515a6 213
theschrade54 0:eae9712515a6 214 comcount = 4; //reset vars
theschrade54 0:eae9712515a6 215 nx = 0;
theschrade54 0:eae9712515a6 216 ny = 0;
theschrade54 0:eae9712515a6 217 moveweight = 0;
theschrade54 0:eae9712515a6 218 dphi = 0;
theschrade54 0:eae9712515a6 219
theschrade54 0:eae9712515a6 220 } else { //else stop
theschrade54 0:eae9712515a6 221 left.stop(1);
theschrade54 0:eae9712515a6 222 right.stop(1);
theschrade54 0:eae9712515a6 223 wait(.1);
theschrade54 0:eae9712515a6 224 }
theschrade54 0:eae9712515a6 225 }
theschrade54 0:eae9712515a6 226 }