Jack Daniels / Mbed 2 deprecated SantaSeeker

IT GOES NORTH MOST OF THE TIME

Dependencies:   HMC6352 Motor mbed

main.cpp@4:9011798d1a20, 2012-10-04 (annotated)

Committer:
vsavkin3
Date:
Thu Oct 04 22:05:11 2012 +0000
Revision:
4:9011798d1a20
Parent:
3:4ee5247f47ec
Child:
5:99545cf4dff4
Robot correctly turns away from obstacles and then moves forward. Can follow a wall! (Kind of)

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 4:9011798d1a20 10
vsavkin3 4:9011798d1a20 11 enum direction{ Right, Forward };
vsavkin3 4:9011798d1a20 12
vsavkin3 4:9011798d1a20 13 int main() {
vsavkin3 4:9011798d1a20 14 enum direction dir=Forward;
vsavkin3 4:9011798d1a20 15
vsavkin3 4:9011798d1a20 16 printf("Front Right Left\n\r");
vsavkin3 4:9011798d1a20 17 int i=0;
vsavkin3 4:9011798d1a20 18 float readLeftVals[10];
vsavkin3 4:9011798d1a20 19 float readRightVals[10];
vsavkin3 4:9011798d1a20 20 float readFrontVals[10];
vsavkin3 4:9011798d1a20 21 float readLeft;
vsavkin3 4:9011798d1a20 22 float readRight;
vsavkin3 4:9011798d1a20 23 float readFront;
vsavkin3 4:9011798d1a20 24
vsavkin3 4:9011798d1a20 25 //Initialize the averager before the robot even moves
vsavkin3 4:9011798d1a20 26 for (int j=0; j<10; j++);
vsavkin3 4:9011798d1a20 27 {
vsavkin3 4:9011798d1a20 28 readLeft=irLeft;
vsavkin3 4:9011798d1a20 29 readLeft=21/readLeft;
vsavkin3 4:9011798d1a20 30
vsavkin3 4:9011798d1a20 31 readRight=irRight;
vsavkin3 4:9011798d1a20 32 readRight=21/readRight;
vsavkin3 4:9011798d1a20 33
vsavkin3 4:9011798d1a20 34 readFront=irFront;
vsavkin3 4:9011798d1a20 35 readFront=21/readFront;
vsavkin3 4:9011798d1a20 36
vsavkin3 4:9011798d1a20 37 readFrontVals[i] = readFront;
vsavkin3 4:9011798d1a20 38 readRightVals[i] = readRight;
vsavkin3 4:9011798d1a20 39 readLeftVals[i] = readLeft;
vsavkin3 4:9011798d1a20 40 }
vsavkin3 4:9011798d1a20 41
vsavkin3 4:9011798d1a20 42 while (1){
vsavkin3 4:9011798d1a20 43 readLeft=irLeft;
vsavkin3 4:9011798d1a20 44 readLeft=21/readLeft;
vsavkin3 4:9011798d1a20 45
vsavkin3 4:9011798d1a20 46 readRight=irRight;
vsavkin3 4:9011798d1a20 47 readRight=21/readRight;
vsavkin3 4:9011798d1a20 48
vsavkin3 4:9011798d1a20 49 readFront=irFront;
vsavkin3 4:9011798d1a20 50 readFront=21/readFront;
vsavkin3 4:9011798d1a20 51
vsavkin3 4:9011798d1a20 52
vsavkin3 4:9011798d1a20 53
vsavkin3 4:9011798d1a20 54 if (i>=10)
vsavkin3 4:9011798d1a20 55 {
vsavkin3 4:9011798d1a20 56 i=0;
vsavkin3 4:9011798d1a20 57 }
vsavkin3 4:9011798d1a20 58 readFrontVals[i] = readFront;
vsavkin3 4:9011798d1a20 59 readRightVals[i] = readRight;
vsavkin3 4:9011798d1a20 60 readLeftVals[i] = readLeft;
vsavkin3 4:9011798d1a20 61 i++;
vsavkin3 4:9011798d1a20 62
vsavkin3 4:9011798d1a20 63 float avgFront = 0, avgRight = 0, avgLeft = 0;
vsavkin3 4:9011798d1a20 64
vsavkin3 4:9011798d1a20 65 for (int j=0; j<10; j++)
vsavkin3 4:9011798d1a20 66 {
vsavkin3 4:9011798d1a20 67 avgFront += readFrontVals[j]/10;
vsavkin3 4:9011798d1a20 68 avgLeft += readLeftVals[j]/10;
vsavkin3 4:9011798d1a20 69 avgRight += readRightVals[j]/10;
vsavkin3 4:9011798d1a20 70 }
vsavkin3 4:9011798d1a20 71 avgFront = readFront;
vsavkin3 4:9011798d1a20 72 avgRight = readRight;
vsavkin3 4:9011798d1a20 73 //printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft);
vsavkin3 4:9011798d1a20 74
vsavkin3 4:9011798d1a20 75 switch(dir){
vsavkin3 4:9011798d1a20 76 case Forward:
vsavkin3 4:9011798d1a20 77 if (avgFront <= 40)
vsavkin3 4:9011798d1a20 78 {
vsavkin3 4:9011798d1a20 79 right.speed(0);
vsavkin3 4:9011798d1a20 80 left.speed(0);
vsavkin3 4:9011798d1a20 81 // printf("stopped!\n\r");
vsavkin3 4:9011798d1a20 82 //wait(0.1);
vsavkin3 4:9011798d1a20 83 dir=Right;
vsavkin3 4:9011798d1a20 84 // printf("less than 18\n\r");
vsavkin3 4:9011798d1a20 85 }
vsavkin3 4:9011798d1a20 86 else
vsavkin3 4:9011798d1a20 87 {
vsavkin3 4:9011798d1a20 88 right.speed(1);
vsavkin3 4:9011798d1a20 89 left.speed(1);
vsavkin3 4:9011798d1a20 90 }
vsavkin3 4:9011798d1a20 91 break;
vsavkin3 4:9011798d1a20 92 case Right:
vsavkin3 4:9011798d1a20 93 if (avgLeft>=50 || avgFront<=40)
vsavkin3 4:9011798d1a20 94 {
vsavkin3 4:9011798d1a20 95 right.speed(-1);
vsavkin3 4:9011798d1a20 96 left.speed(0);
vsavkin3 4:9011798d1a20 97 // printf("turning!\n\r");
vsavkin3 4:9011798d1a20 98 }
vsavkin3 4:9011798d1a20 99 else
vsavkin3 4:9011798d1a20 100 {
vsavkin3 4:9011798d1a20 101 right.speed(0);
vsavkin3 4:9011798d1a20 102 left.speed(0);
vsavkin3 4:9011798d1a20 103 // printf("stopped!\n\r");
vsavkin3 4:9011798d1a20 104 //wait(0.1);
vsavkin3 4:9011798d1a20 105 dir=Forward;
vsavkin3 4:9011798d1a20 106 }
vsavkin3 4:9011798d1a20 107 break;
vsavkin3 4:9011798d1a20 108 }
vsavkin3 4:9011798d1a20 109 wait(0.01);
vsavkin3 4:9011798d1a20 110 }
vsavkin3 4:9011798d1a20 111 }