Vince Salpietro / Mbed 2 deprecated mbed_blinky

shsh

Dependencies:   m3pi_ng mbed

main.cpp@1:53d1b31be014, 2014-06-12 (annotated)

Committer:
vsal
Date:
Thu Jun 12 12:10:06 2014 +0000
Revision:
1:53d1b31be014
Parent:
0:ce0a2659915b 
Final extemum

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 1:53d1b31be014 5 // estimate for the value of PI
vsal 0:ce0a2659915b 6 const double pi = 3.14159;
vsal 0:ce0a2659915b 7
vsal 1:53d1b31be014 8 // initializes robot and timer object
vsal 0:ce0a2659915b 9 m3pi huey;
vsal 1:53d1b31be014 10 Timer timer, samp;
vsal 0:ce0a2659915b 11
vsal 1:53d1b31be014 12 // Puts the sensor value through a high pass filter, cutting off low frequencies, and returning the manipulated gradient value
vsal 1:53d1b31be014 13 double highPass(double yp, int uc, int up, double cutoff, double sampTime)
vsal 0:ce0a2659915b 14 {
vsal 1:53d1b31be014 15 return -abs((1-(sampTime*cutoff))*yp+uc-up);
vsal 0:ce0a2659915b 16 }
vsal 0:ce0a2659915b 17
vsal 1:53d1b31be014 18 // returns a speed based on the calculated gradient, wavelength of oscilation, and the run time
vsal 1:53d1b31be014 19 float angleFormula(double angVel,double &yc, double yp, int uc, int up, float time, double a, double c, double d, double cutoff, double sampTime)
vsal 1:53d1b31be014 20 {
vsal 1:53d1b31be014 21 yc = highPass(yp,uc,up,cutoff,sampTime);
vsal 1:53d1b31be014 22 return a*angVel*cos(angVel*time)+c*yc*sin(angVel*time)-d*yc*yc*sin(angVel*time);
vsal 1:53d1b31be014 23 }
vsal 0:ce0a2659915b 24
vsal 0:ce0a2659915b 25
vsal 0:ce0a2659915b 26 int main()
vsal 0:ce0a2659915b 27 {
vsal 0:ce0a2659915b 28 int z = 1;
vsal 0:ce0a2659915b 29 int sensors[5];
vsal 0:ce0a2659915b 30
vsal 1:53d1b31be014 31 //Sets the speed and angular velocity of the robot
vsal 0:ce0a2659915b 32 float speed = 0.1;
vsal 1:53d1b31be014 33 double freq = 0.5;//.125
vsal 1:53d1b31be014 34 double angVel = 2*pi*freq;
vsal 1:53d1b31be014 35
vsal 0:ce0a2659915b 36 float u;
vsal 0:ce0a2659915b 37
vsal 1:53d1b31be014 38 double cutoff = 0.05; // creates the cutoff frequency for the high pass
vsal 1:53d1b31be014 39 double sampTime = 0.1; // sampling time for the high pass
vsal 1:53d1b31be014 40
vsal 1:53d1b31be014 41 double ycur, yprev = 0; // the current and previous gradient values, initializes previous to 0 for first calculation
vsal 1:53d1b31be014 42 int ucur, uprev = 0; // the current and previous raw sensor values, initializes previous to 0 for first calculation
vsal 0:ce0a2659915b 43
vsal 1:53d1b31be014 44 huey.raw_sensor(sensors);// gets sensor values
vsal 0:ce0a2659915b 45
vsal 1:53d1b31be014 46 double a = .05/angVel,c = 0.00075,d = pow(.0001,2); // initializes 3 constants for the angle formula funciton
vsal 1:53d1b31be014 47 ucur = sensors[2];//sets current sensor value to the raw sensor value
vsal 1:53d1b31be014 48 ycur = highPass(yprev, ucur, uprev, cutoff, sampTime);//calculates initial gradient
vsal 1:53d1b31be014 49 uprev = ucur;// sets previous sensor value to the current sensor value for next calculation
vsal 1:53d1b31be014 50 timer.start();//starts run timer
vsal 1:53d1b31be014 51 samp.start();// starts sampling timer
vsal 0:ce0a2659915b 52 while(z == 1)
vsal 0:ce0a2659915b 53 {
vsal 1:53d1b31be014 54 samp.reset();//resets the sampling time for each iteration
vsal 0:ce0a2659915b 55 huey.raw_sensor(sensors);
vsal 1:53d1b31be014 56 ucur = sensors[2];
vsal 0:ce0a2659915b 57
vsal 1:53d1b31be014 58 huey.cls();
vsal 1:53d1b31be014 59 u = angleFormula(angVel,ycur,yprev,ucur,uprev,timer.read(),a,c,d,cutoff,sampTime);//recieves speed from formula
vsal 1:53d1b31be014 60 huey.printf("%f", u);
vsal 0:ce0a2659915b 61
vsal 1:53d1b31be014 62 //adds u to the speed of the left and right motors to control the direction the robot moves
vsal 0:ce0a2659915b 63 huey.right_motor(speed - u);
vsal 0:ce0a2659915b 64 huey.left_motor(speed + u);
vsal 0:ce0a2659915b 65
vsal 1:53d1b31be014 66 //if(sensors[2] == 2000)
vsal 1:53d1b31be014 67 //break;
vsal 1:53d1b31be014 68 // sets previous values to the current values for next calculation
vsal 1:53d1b31be014 69 yprev = ycur;
vsal 1:53d1b31be014 70 uprev = ucur;
vsal 1:53d1b31be014 71 //waits until the system has fully reached its sampling time
vsal 1:53d1b31be014 72 while((samp.read_ms()) < (sampTime * 1000) )
vsal 1:53d1b31be014 73 {
vsal 1:53d1b31be014 74 }
vsal 0:ce0a2659915b 75 }
vsal 0:ce0a2659915b 76 huey.stop();
vsal 0:ce0a2659915b 77 }