Vince Salpietro / Mbed 2 deprecated mbed_blinky

shsh

Dependencies:   m3pi_ng mbed

main.cpp@0:ce0a2659915b, 2014-06-06 (annotated)

Committer:
vsal
Date:
Fri Jun 06 08:44:31 2014 +0000
Revision:
0:ce0a2659915b
Child:
1:53d1b31be014
Oscilates but does not get closer to dark points

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsal 0:ce0a2659915b 1 #include "mbed.h"
vsal 0:ce0a2659915b 2 #include "m3pi_ng.h"
vsal 0:ce0a2659915b 3 #include "math.h"
vsal 0:ce0a2659915b 4
vsal 0:ce0a2659915b 5 const double pi = 3.14159;
vsal 0:ce0a2659915b 6
vsal 0:ce0a2659915b 7 m3pi huey;
vsal 0:ce0a2659915b 8 Timer timer;
vsal 0:ce0a2659915b 9
vsal 0:ce0a2659915b 10 double average(int* arr)
vsal 0:ce0a2659915b 11 {
vsal 0:ce0a2659915b 12 int sum = 0;
vsal 0:ce0a2659915b 13 for(int i = 0; i < sizeof(arr); i++)
vsal 0:ce0a2659915b 14 {
vsal 0:ce0a2659915b 15 sum = sum + arr[i];
vsal 0:ce0a2659915b 16 }
vsal 0:ce0a2659915b 17 return sum / sizeof(arr);
vsal 0:ce0a2659915b 18 }
vsal 0:ce0a2659915b 19
vsal 0:ce0a2659915b 20 /*
vsal 0:ce0a2659915b 21
vsal 0:ce0a2659915b 22 End goal is to have heuy oscilate side to side and constatly determine which side is greater so that using the
vsal 0:ce0a2659915b 23 right and left motors huey can favor that direction
vsal 0:ce0a2659915b 24 Currently oscilates but does not do anything other than printing the sensor value, does not use the value for anything
vsal 0:ce0a2659915b 25
vsal 0:ce0a2659915b 26 Failed attempt at taking averages
vsal 0:ce0a2659915b 27
vsal 0:ce0a2659915b 28 Attempting to only use two consecutive sensor values as our judges;
vsal 0:ce0a2659915b 29
vsal 0:ce0a2659915b 30 */
vsal 0:ce0a2659915b 31
vsal 0:ce0a2659915b 32 int main()
vsal 0:ce0a2659915b 33 {
vsal 0:ce0a2659915b 34 int z = 1;
vsal 0:ce0a2659915b 35 int sensors[5];
vsal 0:ce0a2659915b 36
vsal 0:ce0a2659915b 37 float speed = 0.1;
vsal 0:ce0a2659915b 38 int freq = 5;
vsal 0:ce0a2659915b 39 float u;
vsal 0:ce0a2659915b 40
vsal 0:ce0a2659915b 41 int s,f;
vsal 0:ce0a2659915b 42 double time;
vsal 0:ce0a2659915b 43 timer.start();
vsal 0:ce0a2659915b 44 s = timer.read_ms();
vsal 0:ce0a2659915b 45
vsal 0:ce0a2659915b 46 //int* right;
vsal 0:ce0a2659915b 47 //int* left;
vsal 0:ce0a2659915b 48 //int r = 0,l = 0;
vsal 0:ce0a2659915b 49 //double avgDif;
vsal 0:ce0a2659915b 50
vsal 0:ce0a2659915b 51 int diff;
vsal 0:ce0a2659915b 52
vsal 0:ce0a2659915b 53 huey.raw_sensor(sensors);
vsal 0:ce0a2659915b 54 int prev = sensors[2];
vsal 0:ce0a2659915b 55 int curr;
vsal 0:ce0a2659915b 56 while(z == 1)
vsal 0:ce0a2659915b 57 {
vsal 0:ce0a2659915b 58 f = timer.read_ms();
vsal 0:ce0a2659915b 59 time = f - s;
vsal 0:ce0a2659915b 60 u = speed*sin(time/(2*pi*freq));
vsal 0:ce0a2659915b 61 huey.raw_sensor(sensors);
vsal 0:ce0a2659915b 62 curr = sensors[2];
vsal 0:ce0a2659915b 63 diff = curr - prev;
vsal 0:ce0a2659915b 64 prev = curr;
vsal 0:ce0a2659915b 65 //u = u + diff/2000;
vsal 0:ce0a2659915b 66 huey.cls();
vsal 0:ce0a2659915b 67 huey.printf("%i", sensors[2]);
vsal 0:ce0a2659915b 68
vsal 0:ce0a2659915b 69 /*
vsal 0:ce0a2659915b 70 Attempting to determine which side of the sensors are greater so that the robot can move in that direction
vsal 0:ce0a2659915b 71 if(u > 0)
vsal 0:ce0a2659915b 72 {
vsal 0:ce0a2659915b 73 right[r] = sensors[2];
vsal 0:ce0a2659915b 74 r++;
vsal 0:ce0a2659915b 75 }
vsal 0:ce0a2659915b 76 else if(u < 0)
vsal 0:ce0a2659915b 77 {
vsal 0:ce0a2659915b 78 left[l] = sensors[2];
vsal 0:ce0a2659915b 79 l++;
vsal 0:ce0a2659915b 80 }
vsal 0:ce0a2659915b 81 avgDif = average(right) - average(left);
vsal 0:ce0a2659915b 82 u = u + avgDif/2000;
vsal 0:ce0a2659915b 83 */
vsal 0:ce0a2659915b 84
vsal 0:ce0a2659915b 85 huey.right_motor(speed - u);
vsal 0:ce0a2659915b 86 huey.left_motor(speed + u);
vsal 0:ce0a2659915b 87
vsal 0:ce0a2659915b 88 if(sensors[2] == 2000)
vsal 0:ce0a2659915b 89 break;
vsal 0:ce0a2659915b 90 //wait(0.01);
vsal 0:ce0a2659915b 91 }
vsal 0:ce0a2659915b 92 huey.stop();
vsal 0:ce0a2659915b 93 }