Jack Daniels / Mbed 2 deprecated SantaSeeker

IT GOES NORTH MOST OF THE TIME

Dependencies:   HMC6352 Motor mbed

main.cpp@5:99545cf4dff4, 2012-10-04 (annotated)

Committer:
vsavkin3
Date:
Thu Oct 04 22:05:37 2012 +0000
Revision:
5:99545cf4dff4
Parent:
4:9011798d1a20
Child:
6:2b89c05628ce
Compass interfaced with the robot. Heading can be read.

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 4:9011798d1a20 12 enum direction{ Right, Forward };
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 printf("Heading: %f\n\r", compass.sample()/10.0);
vsavkin3 5:99545cf4dff4 56
vsavkin3 4:9011798d1a20 57
vsavkin3 4:9011798d1a20 58
vsavkin3 4:9011798d1a20 59 if (i>=10)
vsavkin3 4:9011798d1a20 60 {
vsavkin3 4:9011798d1a20 61 i=0;
vsavkin3 4:9011798d1a20 62 }
vsavkin3 4:9011798d1a20 63 readFrontVals[i] = readFront;
vsavkin3 4:9011798d1a20 64 readRightVals[i] = readRight;
vsavkin3 4:9011798d1a20 65 readLeftVals[i] = readLeft;
vsavkin3 4:9011798d1a20 66 i++;
vsavkin3 4:9011798d1a20 67
vsavkin3 4:9011798d1a20 68 float avgFront = 0, avgRight = 0, avgLeft = 0;
vsavkin3 4:9011798d1a20 69
vsavkin3 4:9011798d1a20 70 for (int j=0; j<10; j++)
vsavkin3 4:9011798d1a20 71 {
vsavkin3 4:9011798d1a20 72 avgFront += readFrontVals[j]/10;
vsavkin3 4:9011798d1a20 73 avgLeft += readLeftVals[j]/10;
vsavkin3 4:9011798d1a20 74 avgRight += readRightVals[j]/10;
vsavkin3 4:9011798d1a20 75 }
vsavkin3 4:9011798d1a20 76 avgFront = readFront;
vsavkin3 4:9011798d1a20 77 avgRight = readRight;
vsavkin3 4:9011798d1a20 78 //printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft);
vsavkin3 4:9011798d1a20 79
vsavkin3 4:9011798d1a20 80 switch(dir){
vsavkin3 4:9011798d1a20 81 case Forward:
vsavkin3 4:9011798d1a20 82 if (avgFront <= 40)
vsavkin3 4:9011798d1a20 83 {
vsavkin3 4:9011798d1a20 84 right.speed(0);
vsavkin3 4:9011798d1a20 85 left.speed(0);
vsavkin3 4:9011798d1a20 86 // printf("stopped!\n\r");
vsavkin3 4:9011798d1a20 87 //wait(0.1);
vsavkin3 4:9011798d1a20 88 dir=Right;
vsavkin3 4:9011798d1a20 89 // printf("less than 18\n\r");
vsavkin3 4:9011798d1a20 90 }
vsavkin3 4:9011798d1a20 91 else
vsavkin3 4:9011798d1a20 92 {
vsavkin3 4:9011798d1a20 93 right.speed(1);
vsavkin3 4:9011798d1a20 94 left.speed(1);
vsavkin3 4:9011798d1a20 95 }
vsavkin3 4:9011798d1a20 96 break;
vsavkin3 4:9011798d1a20 97 case Right:
vsavkin3 4:9011798d1a20 98 if (avgLeft>=50 || avgFront<=40)
vsavkin3 4:9011798d1a20 99 {
vsavkin3 4:9011798d1a20 100 right.speed(-1);
vsavkin3 4:9011798d1a20 101 left.speed(0);
vsavkin3 4:9011798d1a20 102 // printf("turning!\n\r");
vsavkin3 4:9011798d1a20 103 }
vsavkin3 4:9011798d1a20 104 else
vsavkin3 4:9011798d1a20 105 {
vsavkin3 4:9011798d1a20 106 right.speed(0);
vsavkin3 4:9011798d1a20 107 left.speed(0);
vsavkin3 4:9011798d1a20 108 // printf("stopped!\n\r");
vsavkin3 4:9011798d1a20 109 //wait(0.1);
vsavkin3 4:9011798d1a20 110 dir=Forward;
vsavkin3 4:9011798d1a20 111 }
vsavkin3 4:9011798d1a20 112 break;
vsavkin3 4:9011798d1a20 113 }
vsavkin3 4:9011798d1a20 114 wait(0.01);
vsavkin3 4:9011798d1a20 115 }
vsavkin3 4:9011798d1a20 116 }