Jack Daniels / Mbed 2 deprecated SantaSeeker

IT GOES NORTH MOST OF THE TIME

Dependencies:   HMC6352 Motor mbed

main.cpp@3:4ee5247f47ec, 2012-10-04 (annotated)

Committer:
vsavkin3
Date:
Thu Oct 04 20:45:34 2012 +0000
Revision:
3:4ee5247f47ec
Parent:
1:6812f830e5a1
Child:
4:9011798d1a20
Change calibration and fixed some bugs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsavkin3 0:1a0fa585e36c 1 #include "mbed.h"
vsavkin3 0:1a0fa585e36c 2 #include "Motor.h"
vsavkin3 0:1a0fa585e36c 3
vsavkin3 0:1a0fa585e36c 4 Motor right(p21, p23, p22); // pwm, fwd, rev
vsavkin3 0:1a0fa585e36c 5 Motor left(p26, p24, p25); // pwm, fwd, rev
vsavkin3 1:6812f830e5a1 6 AnalogIn irLeft(p20);
vsavkin3 1:6812f830e5a1 7 AnalogIn irFront(p19);
vsavkin3 1:6812f830e5a1 8 AnalogIn irRight(p18);
vsavkin3 1:6812f830e5a1 9 AnalogIn sonarLeft(p17);
vsavkin3 1:6812f830e5a1 10
vsavkin3 1:6812f830e5a1 11 enum direction{ Right, Forward };
vsavkin3 0:1a0fa585e36c 12
vsavkin3 0:1a0fa585e36c 13 int main() {
vsavkin3 1:6812f830e5a1 14 enum direction dir=Forward;
vsavkin3 1:6812f830e5a1 15
vsavkin3 1:6812f830e5a1 16 printf("Front Right Left\n\r");
vsavkin3 1:6812f830e5a1 17 int i=0;
vsavkin3 3:4ee5247f47ec 18 float readLeftVals[10];
vsavkin3 3:4ee5247f47ec 19 float readRightVals[10];
vsavkin3 3:4ee5247f47ec 20 float readFrontVals[10];
vsavkin3 1:6812f830e5a1 21 float readLeft;
vsavkin3 1:6812f830e5a1 22 float readRight;
vsavkin3 1:6812f830e5a1 23 float readFront;
vsavkin3 1:6812f830e5a1 24
vsavkin3 1:6812f830e5a1 25 //Initialize the averager before the robot even moves
vsavkin3 1:6812f830e5a1 26 for (int j=0; j<10; j++);
vsavkin3 1:6812f830e5a1 27 {
vsavkin3 1:6812f830e5a1 28 readLeft=irLeft;
vsavkin3 3:4ee5247f47ec 29 readLeft=21/readLeft;
vsavkin3 1:6812f830e5a1 30
vsavkin3 1:6812f830e5a1 31 readRight=irRight;
vsavkin3 3:4ee5247f47ec 32 readRight=21/readRight;
vsavkin3 1:6812f830e5a1 33
vsavkin3 1:6812f830e5a1 34 readFront=irFront;
vsavkin3 3:4ee5247f47ec 35 readFront=21/readFront;
vsavkin3 1:6812f830e5a1 36
vsavkin3 1:6812f830e5a1 37 readFrontVals[i] = readFront;
vsavkin3 1:6812f830e5a1 38 readRightVals[i] = readRight;
vsavkin3 1:6812f830e5a1 39 readLeftVals[i] = readLeft;
vsavkin3 1:6812f830e5a1 40 }
vsavkin3 1:6812f830e5a1 41
vsavkin3 0:1a0fa585e36c 42 while (1){
vsavkin3 1:6812f830e5a1 43 readLeft=irLeft;
vsavkin3 3:4ee5247f47ec 44 readLeft=21/readLeft;
vsavkin3 1:6812f830e5a1 45
vsavkin3 1:6812f830e5a1 46 readRight=irRight;
vsavkin3 3:4ee5247f47ec 47 readRight=21/readRight;
vsavkin3 1:6812f830e5a1 48
vsavkin3 1:6812f830e5a1 49 readFront=irFront;
vsavkin3 3:4ee5247f47ec 50 readFront=21/readFront;
vsavkin3 1:6812f830e5a1 51
vsavkin3 1:6812f830e5a1 52
vsavkin3 1:6812f830e5a1 53
vsavkin3 1:6812f830e5a1 54
vsavkin3 3:4ee5247f47ec 55 if (i>=10)
vsavkin3 1:6812f830e5a1 56 {
vsavkin3 1:6812f830e5a1 57 i=0;
vsavkin3 1:6812f830e5a1 58 }
vsavkin3 1:6812f830e5a1 59 readFrontVals[i] = readFront;
vsavkin3 1:6812f830e5a1 60 readRightVals[i] = readRight;
vsavkin3 1:6812f830e5a1 61 readLeftVals[i] = readLeft;
vsavkin3 1:6812f830e5a1 62 i++;
vsavkin3 1:6812f830e5a1 63
vsavkin3 3:4ee5247f47ec 64 float avgFront = 0, avgRight = 0, avgLeft = 0;
vsavkin3 1:6812f830e5a1 65
vsavkin3 1:6812f830e5a1 66 for (int j=0; j<10; j++)
vsavkin3 1:6812f830e5a1 67 {
vsavkin3 1:6812f830e5a1 68 avgFront += readFrontVals[j]/10;
vsavkin3 1:6812f830e5a1 69 avgLeft += readLeftVals[j]/10;
vsavkin3 1:6812f830e5a1 70 avgRight += readRightVals[j]/10;
vsavkin3 0:1a0fa585e36c 71 }
vsavkin3 1:6812f830e5a1 72
vsavkin3 1:6812f830e5a1 73 printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft);
vsavkin3 1:6812f830e5a1 74
vsavkin3 1:6812f830e5a1 75 switch(dir){
vsavkin3 1:6812f830e5a1 76 case Forward:
vsavkin3 1:6812f830e5a1 77 if (avgFront <= 18)
vsavkin3 1:6812f830e5a1 78 {
vsavkin3 1:6812f830e5a1 79 wait(0.5);
vsavkin3 1:6812f830e5a1 80 dir=Right;
vsavkin3 1:6812f830e5a1 81 printf("less than 18\n\r");
vsavkin3 1:6812f830e5a1 82 }
vsavkin3 1:6812f830e5a1 83 else
vsavkin3 1:6812f830e5a1 84 {
vsavkin3 1:6812f830e5a1 85 right.speed(1);
vsavkin3 1:6812f830e5a1 86 left.speed(1);
vsavkin3 1:6812f830e5a1 87 }
vsavkin3 1:6812f830e5a1 88 break;
vsavkin3 1:6812f830e5a1 89 case Right:
vsavkin3 1:6812f830e5a1 90 if (avgLeft>30 || avgFront<=18)
vsavkin3 1:6812f830e5a1 91 {
vsavkin3 1:6812f830e5a1 92 right.speed(-1);
vsavkin3 1:6812f830e5a1 93 left.speed(1);
vsavkin3 1:6812f830e5a1 94 printf("turning!\n\r");
vsavkin3 1:6812f830e5a1 95 }
vsavkin3 1:6812f830e5a1 96 else
vsavkin3 1:6812f830e5a1 97 {
vsavkin3 1:6812f830e5a1 98 wait(0.5);
vsavkin3 1:6812f830e5a1 99 dir=Forward;
vsavkin3 1:6812f830e5a1 100 }
vsavkin3 1:6812f830e5a1 101 break;
vsavkin3 1:6812f830e5a1 102 }
vsavkin3 1:6812f830e5a1 103 wait(0.01);
vsavkin3 0:1a0fa585e36c 104 }
vsavkin3 0:1a0fa585e36c 105 }