Jack Daniels / Mbed 2 deprecated SantaSeeker

IT GOES NORTH MOST OF THE TIME

Dependencies:   HMC6352 Motor mbed

main.cpp@6:2b89c05628ce, 2012-10-04 (annotated)

Committer:
vsavkin3
Date:
Thu Oct 04 22:53:42 2012 +0000
Revision:
6:2b89c05628ce
Parent:
5:99545cf4dff4
Child:
7:60e5e064698d
Bot successfully went around an obstacle and turned north! Once... good stopping point, however :D

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsavkin3 4:9011798d1a20 1 #include "mbed.h"
vsavkin3 4:9011798d1a20 2 #include "Motor.h"
vsavkin3 4:9011798d1a20 3 #include "HMC6352.h"
vsavkin3 4:9011798d1a20 4
vsavkin3 4:9011798d1a20 5 Motor right(p21, p23, p22); // pwm, fwd, rev
vsavkin3 4:9011798d1a20 6 Motor left(p26, p24, p25); // pwm, fwd, rev
vsavkin3 4:9011798d1a20 7 AnalogIn irLeft(p20);
vsavkin3 4:9011798d1a20 8 AnalogIn irFront(p19);
vsavkin3 4:9011798d1a20 9 AnalogIn irRight(p18);
vsavkin3 5:99545cf4dff4 10 HMC6352 compass(p9, p10);
vsavkin3 4:9011798d1a20 11
vsavkin3 6:2b89c05628ce 12 enum direction{ Right, Forward, Left, TurnNorth };
vsavkin3 4:9011798d1a20 13
vsavkin3 4:9011798d1a20 14 int main() {
vsavkin3 4:9011798d1a20 15 enum direction dir=Forward;
vsavkin3 5:99545cf4dff4 16 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
vsavkin3 4:9011798d1a20 17
vsavkin3 4:9011798d1a20 18 printf("Front Right Left\n\r");
vsavkin3 4:9011798d1a20 19 int i=0;
vsavkin3 4:9011798d1a20 20 float readLeftVals[10];
vsavkin3 4:9011798d1a20 21 float readRightVals[10];
vsavkin3 4:9011798d1a20 22 float readFrontVals[10];
vsavkin3 4:9011798d1a20 23 float readLeft;
vsavkin3 4:9011798d1a20 24 float readRight;
vsavkin3 4:9011798d1a20 25 float readFront;
vsavkin3 4:9011798d1a20 26
vsavkin3 4:9011798d1a20 27 //Initialize the averager before the robot even moves
vsavkin3 4:9011798d1a20 28 for (int j=0; j<10; j++);
vsavkin3 4:9011798d1a20 29 {
vsavkin3 4:9011798d1a20 30 readLeft=irLeft;
vsavkin3 4:9011798d1a20 31 readLeft=21/readLeft;
vsavkin3 4:9011798d1a20 32
vsavkin3 4:9011798d1a20 33 readRight=irRight;
vsavkin3 4:9011798d1a20 34 readRight=21/readRight;
vsavkin3 4:9011798d1a20 35
vsavkin3 4:9011798d1a20 36 readFront=irFront;
vsavkin3 4:9011798d1a20 37 readFront=21/readFront;
vsavkin3 4:9011798d1a20 38
vsavkin3 4:9011798d1a20 39 readFrontVals[i] = readFront;
vsavkin3 4:9011798d1a20 40 readRightVals[i] = readRight;
vsavkin3 4:9011798d1a20 41 readLeftVals[i] = readLeft;
vsavkin3 4:9011798d1a20 42 }
vsavkin3 4:9011798d1a20 43
vsavkin3 4:9011798d1a20 44 while (1){
vsavkin3 4:9011798d1a20 45 readLeft=irLeft;
vsavkin3 4:9011798d1a20 46 readLeft=21/readLeft;
vsavkin3 4:9011798d1a20 47
vsavkin3 4:9011798d1a20 48 readRight=irRight;
vsavkin3 4:9011798d1a20 49 readRight=21/readRight;
vsavkin3 4:9011798d1a20 50
vsavkin3 4:9011798d1a20 51 readFront=irFront;
vsavkin3 4:9011798d1a20 52 readFront=21/readFront;
vsavkin3 4:9011798d1a20 53
vsavkin3 5:99545cf4dff4 54
vsavkin3 5:99545cf4dff4 55
vsavkin3 4:9011798d1a20 56
vsavkin3 4:9011798d1a20 57
vsavkin3 4:9011798d1a20 58 if (i>=10)
vsavkin3 4:9011798d1a20 59 {
vsavkin3 4:9011798d1a20 60 i=0;
vsavkin3 4:9011798d1a20 61 }
vsavkin3 4:9011798d1a20 62 readFrontVals[i] = readFront;
vsavkin3 4:9011798d1a20 63 readRightVals[i] = readRight;
vsavkin3 4:9011798d1a20 64 readLeftVals[i] = readLeft;
vsavkin3 4:9011798d1a20 65 i++;
vsavkin3 4:9011798d1a20 66
vsavkin3 4:9011798d1a20 67 float avgFront = 0, avgRight = 0, avgLeft = 0;
vsavkin3 4:9011798d1a20 68
vsavkin3 4:9011798d1a20 69 for (int j=0; j<10; j++)
vsavkin3 4:9011798d1a20 70 {
vsavkin3 4:9011798d1a20 71 avgFront += readFrontVals[j]/10;
vsavkin3 4:9011798d1a20 72 avgLeft += readLeftVals[j]/10;
vsavkin3 4:9011798d1a20 73 avgRight += readRightVals[j]/10;
vsavkin3 4:9011798d1a20 74 }
vsavkin3 4:9011798d1a20 75 avgFront = readFront;
vsavkin3 4:9011798d1a20 76 avgRight = readRight;
vsavkin3 6:2b89c05628ce 77 printf("Front: %5f Right: %5f Left: %5f", avgFront, avgRight, avgLeft);
vsavkin3 6:2b89c05628ce 78 float globalDir = compass.sample()/10.0;
vsavkin3 6:2b89c05628ce 79 printf(" Heading: %f\n\r", globalDir);
vsavkin3 4:9011798d1a20 80
vsavkin3 4:9011798d1a20 81 switch(dir){
vsavkin3 4:9011798d1a20 82 case Forward:
vsavkin3 6:2b89c05628ce 83 printf("CHARGE!n\r");
vsavkin3 4:9011798d1a20 84 if (avgFront <= 40)
vsavkin3 4:9011798d1a20 85 {
vsavkin3 4:9011798d1a20 86 right.speed(0);
vsavkin3 4:9011798d1a20 87 left.speed(0);
vsavkin3 6:2b89c05628ce 88 printf("stopped!\n\r");
vsavkin3 4:9011798d1a20 89 //wait(0.1);
vsavkin3 4:9011798d1a20 90 dir=Right;
vsavkin3 4:9011798d1a20 91 }
vsavkin3 4:9011798d1a20 92 else
vsavkin3 4:9011798d1a20 93 {
vsavkin3 6:2b89c05628ce 94 if (globalDir > 45 && globalDir <= 135)
vsavkin3 6:2b89c05628ce 95 {
vsavkin3 6:2b89c05628ce 96 //East
vsavkin3 6:2b89c05628ce 97 if (avgLeft >= 50)
vsavkin3 6:2b89c05628ce 98 {
vsavkin3 6:2b89c05628ce 99 right.speed(0);
vsavkin3 6:2b89c05628ce 100 left.speed(0);
vsavkin3 6:2b89c05628ce 101 dir = TurnNorth;
vsavkin3 6:2b89c05628ce 102 }
vsavkin3 6:2b89c05628ce 103 else
vsavkin3 6:2b89c05628ce 104 {
vsavkin3 6:2b89c05628ce 105 right.speed(1);
vsavkin3 6:2b89c05628ce 106 left.speed(1);
vsavkin3 6:2b89c05628ce 107 }
vsavkin3 6:2b89c05628ce 108 }
vsavkin3 6:2b89c05628ce 109 else if (globalDir > 135 && globalDir <= 225)
vsavkin3 6:2b89c05628ce 110 {
vsavkin3 6:2b89c05628ce 111 //It's going south O.o
vsavkin3 6:2b89c05628ce 112 right.speed(1);
vsavkin3 6:2b89c05628ce 113 left.speed(1);
vsavkin3 6:2b89c05628ce 114 }
vsavkin3 6:2b89c05628ce 115 else if (globalDir > 225 && globalDir <= 315)
vsavkin3 6:2b89c05628ce 116 {
vsavkin3 6:2b89c05628ce 117 //West
vsavkin3 6:2b89c05628ce 118 if (avgRight >= 50)
vsavkin3 6:2b89c05628ce 119 {
vsavkin3 6:2b89c05628ce 120 right.speed(0);
vsavkin3 6:2b89c05628ce 121 left.speed(0);
vsavkin3 6:2b89c05628ce 122 dir = TurnNorth;
vsavkin3 6:2b89c05628ce 123 }
vsavkin3 6:2b89c05628ce 124 else
vsavkin3 6:2b89c05628ce 125 {
vsavkin3 6:2b89c05628ce 126 right.speed(1);
vsavkin3 6:2b89c05628ce 127 left.speed(1);
vsavkin3 6:2b89c05628ce 128 }
vsavkin3 6:2b89c05628ce 129 }
vsavkin3 6:2b89c05628ce 130 else
vsavkin3 6:2b89c05628ce 131 {
vsavkin3 6:2b89c05628ce 132 //It's going north!
vsavkin3 6:2b89c05628ce 133 right.speed(1);
vsavkin3 6:2b89c05628ce 134 left.speed(1);
vsavkin3 6:2b89c05628ce 135 }
vsavkin3 4:9011798d1a20 136 }
vsavkin3 4:9011798d1a20 137 break;
vsavkin3 4:9011798d1a20 138 case Right:
vsavkin3 6:2b89c05628ce 139 printf("TURN!\n\r");
vsavkin3 6:2b89c05628ce 140 if (/*avgLeft>=50 ||*/ avgFront<=40)
vsavkin3 4:9011798d1a20 141 {
vsavkin3 4:9011798d1a20 142 right.speed(-1);
vsavkin3 4:9011798d1a20 143 left.speed(0);
vsavkin3 4:9011798d1a20 144 // printf("turning!\n\r");
vsavkin3 4:9011798d1a20 145 }
vsavkin3 4:9011798d1a20 146 else
vsavkin3 4:9011798d1a20 147 {
vsavkin3 4:9011798d1a20 148 right.speed(0);
vsavkin3 4:9011798d1a20 149 left.speed(0);
vsavkin3 4:9011798d1a20 150 // printf("stopped!\n\r");
vsavkin3 4:9011798d1a20 151 //wait(0.1);
vsavkin3 4:9011798d1a20 152 dir=Forward;
vsavkin3 4:9011798d1a20 153 }
vsavkin3 4:9011798d1a20 154 break;
vsavkin3 6:2b89c05628ce 155 case TurnNorth:
vsavkin3 6:2b89c05628ce 156 printf("TO THE NORTH!\n\r");
vsavkin3 6:2b89c05628ce 157 if (globalDir>330 || globalDir < 30)
vsavkin3 6:2b89c05628ce 158 {
vsavkin3 6:2b89c05628ce 159 //It's going north!!
vsavkin3 6:2b89c05628ce 160 right.speed(1);
vsavkin3 6:2b89c05628ce 161 left.speed(1);
vsavkin3 6:2b89c05628ce 162 dir=Forward;
vsavkin3 6:2b89c05628ce 163 }
vsavkin3 6:2b89c05628ce 164 else
vsavkin3 6:2b89c05628ce 165 {
vsavkin3 6:2b89c05628ce 166 right.speed(.5);
vsavkin3 6:2b89c05628ce 167 left.speed(-.5);
vsavkin3 6:2b89c05628ce 168 }
vsavkin3 6:2b89c05628ce 169 break;
vsavkin3 4:9011798d1a20 170 }
vsavkin3 4:9011798d1a20 171 wait(0.01);
vsavkin3 4:9011798d1a20 172 }
vsavkin3 4:9011798d1a20 173 }