Jack Daniels / Mbed 2 deprecated SantaSeeker

IT GOES NORTH MOST OF THE TIME

Dependencies:   HMC6352 Motor mbed

main.cpp@2:25d2332ec855, 2012-10-04 (annotated)

Committer:
vsavkin3
Date:
Thu Oct 04 20:42:07 2012 +0000
Revision:
2:25d2332ec855
Parent:
1:6812f830e5a1 
Trying the raw output of the IR sensors

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 2:25d2332ec855 18 float readLeftVals[10];
vsavkin3 2:25d2332ec855 19 float readRightVals[10];
vsavkin3 2:25d2332ec855 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 2:25d2332ec855 28 readFront=irFront;
vsavkin3 1:6812f830e5a1 29 readLeft=irLeft;
vsavkin3 1:6812f830e5a1 30 readRight=irRight;
vsavkin3 2:25d2332ec855 31 readLeft=readLeft*3.3/.0032;
vsavkin3 2:25d2332ec855 32
vsavkin3 1:6812f830e5a1 33
vsavkin3 2:25d2332ec855 34 readRight=readRight*3.3/.0032;
vsavkin3 2:25d2332ec855 35
vsavkin3 2:25d2332ec855 36
vsavkin3 2:25d2332ec855 37 readFront=readFront*3.3/.0032;
vsavkin3 1:6812f830e5a1 38
vsavkin3 1:6812f830e5a1 39 readFrontVals[i] = readFront;
vsavkin3 1:6812f830e5a1 40 readRightVals[i] = readRight;
vsavkin3 1:6812f830e5a1 41 readLeftVals[i] = readLeft;
vsavkin3 1:6812f830e5a1 42 }
vsavkin3 1:6812f830e5a1 43
vsavkin3 0:1a0fa585e36c 44 while (1){
vsavkin3 1:6812f830e5a1 45 readLeft=irLeft;
vsavkin3 2:25d2332ec855 46 readRight=irRight;
vsavkin3 2:25d2332ec855 47 readFront=irFront;
vsavkin3 2:25d2332ec855 48 readLeft=readLeft*3.3/.0032;
vsavkin3 2:25d2332ec855 49
vsavkin3 1:6812f830e5a1 50
vsavkin3 2:25d2332ec855 51 readRight=readRight*3.3/.0032;
vsavkin3 1:6812f830e5a1 52
vsavkin3 2:25d2332ec855 53
vsavkin3 2:25d2332ec855 54 readFront=readFront*3.3/.0032;
vsavkin3 1:6812f830e5a1 55
vsavkin3 1:6812f830e5a1 56
vsavkin3 1:6812f830e5a1 57
vsavkin3 1:6812f830e5a1 58
vsavkin3 2:25d2332ec855 59 if (i>=10)
vsavkin3 1:6812f830e5a1 60 {
vsavkin3 1:6812f830e5a1 61 i=0;
vsavkin3 1:6812f830e5a1 62 }
vsavkin3 1:6812f830e5a1 63 readFrontVals[i] = readFront;
vsavkin3 1:6812f830e5a1 64 readRightVals[i] = readRight;
vsavkin3 1:6812f830e5a1 65 readLeftVals[i] = readLeft;
vsavkin3 1:6812f830e5a1 66 i++;
vsavkin3 1:6812f830e5a1 67
vsavkin3 2:25d2332ec855 68 float avgFront = 0, avgRight = 0, avgLeft = 0;
vsavkin3 1:6812f830e5a1 69
vsavkin3 1:6812f830e5a1 70 for (int j=0; j<10; j++)
vsavkin3 1:6812f830e5a1 71 {
vsavkin3 1:6812f830e5a1 72 avgFront += readFrontVals[j]/10;
vsavkin3 1:6812f830e5a1 73 avgLeft += readLeftVals[j]/10;
vsavkin3 1:6812f830e5a1 74 avgRight += readRightVals[j]/10;
vsavkin3 0:1a0fa585e36c 75 }
vsavkin3 1:6812f830e5a1 76
vsavkin3 1:6812f830e5a1 77 printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft);
vsavkin3 1:6812f830e5a1 78
vsavkin3 2:25d2332ec855 79 avgFront = readFront;
vsavkin3 2:25d2332ec855 80 avgLeft = readLeft;
vsavkin3 1:6812f830e5a1 81 switch(dir){
vsavkin3 1:6812f830e5a1 82 case Forward:
vsavkin3 2:25d2332ec855 83 if (avgFront >= 520)
vsavkin3 1:6812f830e5a1 84 {
vsavkin3 2:25d2332ec855 85 left.speed(0);
vsavkin3 2:25d2332ec855 86 right.speed(0);
vsavkin3 2:25d2332ec855 87 dir=Right;
vsavkin3 1:6812f830e5a1 88 wait(0.5);
vsavkin3 1:6812f830e5a1 89 printf("less than 18\n\r");
vsavkin3 1:6812f830e5a1 90 }
vsavkin3 1:6812f830e5a1 91 else
vsavkin3 1:6812f830e5a1 92 {
vsavkin3 1:6812f830e5a1 93 right.speed(1);
vsavkin3 1:6812f830e5a1 94 left.speed(1);
vsavkin3 1:6812f830e5a1 95 }
vsavkin3 1:6812f830e5a1 96 break;
vsavkin3 1:6812f830e5a1 97 case Right:
vsavkin3 2:25d2332ec855 98 if (avgFront >= 470 || avgLeft<=510)
vsavkin3 2:25d2332ec855 99 { //if(avgFront <=480){dir=Forward; wait(0.5);}
vsavkin3 1:6812f830e5a1 100 right.speed(-1);
vsavkin3 1:6812f830e5a1 101 left.speed(1);
vsavkin3 1:6812f830e5a1 102 printf("turning!\n\r");
vsavkin3 2:25d2332ec855 103
vsavkin3 1:6812f830e5a1 104 }
vsavkin3 2:25d2332ec855 105 else
vsavkin3 1:6812f830e5a1 106 {
vsavkin3 2:25d2332ec855 107
vsavkin3 2:25d2332ec855 108 left.speed(0);
vsavkin3 2:25d2332ec855 109 right.speed(0);
vsavkin3 1:6812f830e5a1 110 wait(0.5);
vsavkin3 1:6812f830e5a1 111 dir=Forward;
vsavkin3 1:6812f830e5a1 112 }
vsavkin3 1:6812f830e5a1 113 break;
vsavkin3 1:6812f830e5a1 114 }
vsavkin3 1:6812f830e5a1 115 wait(0.01);
vsavkin3 0:1a0fa585e36c 116 }
vsavkin3 0:1a0fa585e36c 117 }