der Roboter / Mbed 2 deprecated VariablesGradient

Explanation of Variables

Dependencies:   m3pi_ng mbed

Fork of BlueToothStuff by der Roboter

main.cpp@4:db3011440b4f, 2014-06-10 (annotated)

Committer:
bayagich
Date:
Tue Jun 10 08:07:42 2014 +0000
Revision:
4:db3011440b4f
Parent:
3:ac07a11318f9
Child:
5:c91ef337eff4
Calibrating then not doing anything...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bayagich 2:acc52907bcce 1 //This will define all variables needed for the gradient equation
bayagich 3:ac07a11318f9 2 // new angular frequency EQUATION: w(new) = a*w*cost(w*t) + c*sensor2*sin(wt) - d*(sensor2^2)*sin(wt)
bayagich 3:ac07a11318f9 3 //high pass EQUATION: altsens = mult*prevaltsens + sensor2 - prevsensor
bayagich 3:ac07a11318f9 4 //mult = 1 - t*w
bayagich 2:acc52907bcce 5 //u is modified by the w found, u = cos(wt)
bayagich 2:acc52907bcce 6 //leftspeed = left + u
bayagich 2:acc52907bcce 7 //rightspeed = right - u
bayagich 2:acc52907bcce 8
mmpeter 0:386c250325ce 9 #include "mbed.h"
mmpeter 0:386c250325ce 10 #include "m3pi_ng.h"
mmpeter 0:386c250325ce 11 #include "cmath"
mmpeter 0:386c250325ce 12 #include "iostream"
satyson 1:6402638c6f6d 13 #include "btbee.h"
mmpeter 0:386c250325ce 14
mmpeter 0:386c250325ce 15 using namespace std;
mmpeter 0:386c250325ce 16
mmpeter 0:386c250325ce 17 m3pi thinggy;
satyson 1:6402638c6f6d 18 btbee btbee;
mmpeter 0:386c250325ce 19
mmpeter 0:386c250325ce 20 int main() {
bayagich 2:acc52907bcce 21
bayagich 3:ac07a11318f9 22 int sensor[5];
bayagich 3:ac07a11318f9 23
bayagich 3:ac07a11318f9 24 //w = angular frequency, which is modified after you take each time
bayagich 3:ac07a11318f9 25 double angf = 0;
bayagich 2:acc52907bcce 26
bayagich 3:ac07a11318f9 27 //t = how frequently you change w
bayagich 3:ac07a11318f9 28 //should be between 0.001 and 0.01
bayagich 3:ac07a11318f9 29 double t = 0.001;
bayagich 2:acc52907bcce 30
bayagich 3:ac07a11318f9 31 //variables in the equation to calculate angular frequency
bayagich 4:db3011440b4f 32 double a = .01;
bayagich 4:db3011440b4f 33 double c = .01;
bayagich 4:db3011440b4f 34 double d = .01;
bayagich 2:acc52907bcce 35
bayagich 3:ac07a11318f9 36 //the middle sensor value you take every t seconds
bayagich 3:ac07a11318f9 37 double sensor2;
bayagich 3:ac07a11318f9 38 //you need the previous sensor for the high pass equation
bayagich 3:ac07a11318f9 39 double prevsensor;
bayagich 2:acc52907bcce 40
bayagich 3:ac07a11318f9 41 //this u alters left speed and right speed
bayagich 3:ac07a11318f9 42 double u;
bayagich 3:ac07a11318f9 43 float leftspeed = .01;
bayagich 3:ac07a11318f9 44 float rightspeed = .01;
bayagich 2:acc52907bcce 45
bayagich 3:ac07a11318f9 46 //when you pass the sensor values through high pass
bayagich 3:ac07a11318f9 47 //altered sensor
bayagich 3:ac07a11318f9 48 double altsens;
bayagich 3:ac07a11318f9 49 //previous altered sensor
bayagich 3:ac07a11318f9 50 double prevaltsens = 0;
mmpeter 0:386c250325ce 51
bayagich 4:db3011440b4f 52 thinggy.sensor_auto_calibrate();
bayagich 4:db3011440b4f 53
bayagich 3:ac07a11318f9 54 //read in sensor value
bayagich 4:db3011440b4f 55 thinggy.calibrated_sensor(sensor);
bayagich 3:ac07a11318f9 56 sensor2 = sensor[2];
bayagich 2:acc52907bcce 57
bayagich 3:ac07a11318f9 58 //variable if the best point is found
bayagich 3:ac07a11318f9 59 int found = 0;
mmpeter 0:386c250325ce 60
bayagich 3:ac07a11318f9 61 //loop to change w as it picks up values
bayagich 3:ac07a11318f9 62 while(!found){
bayagich 2:acc52907bcce 63
bayagich 4:db3011440b4f 64 if(leftspeed >= 1 and rightspeed >= 1){
bayagich 4:db3011440b4f 65 thinggy.right_motor(1);
bayagich 4:db3011440b4f 66 thinggy.left_motor(1);
bayagich 4:db3011440b4f 67 }
bayagich 4:db3011440b4f 68 else if(leftspeed >= 1){
bayagich 4:db3011440b4f 69 thinggy.left_motor(1);
bayagich 4:db3011440b4f 70 thinggy.right_motor(rightspeed);
bayagich 4:db3011440b4f 71 }
bayagich 4:db3011440b4f 72 else if (rightspeed >= 1){
bayagich 4:db3011440b4f 73 thinggy.right_motor(1);
bayagich 4:db3011440b4f 74 thinggy.left_motor(leftspeed);
bayagich 4:db3011440b4f 75 }
bayagich 4:db3011440b4f 76 else {
bayagich 4:db3011440b4f 77 //move robot forward for a certain amount of time
bayagich 4:db3011440b4f 78 thinggy.right_motor(leftspeed);
bayagich 4:db3011440b4f 79 thinggy.left_motor(rightspeed);
bayagich 4:db3011440b4f 80 }
bayagich 3:ac07a11318f9 81
bayagich 4:db3011440b4f 82 wait(50);
bayagich 3:ac07a11318f9 83
bayagich 3:ac07a11318f9 84 prevsensor = sensor2;
bayagich 3:ac07a11318f9 85 thinggy.raw_sensor(sensor);
bayagich 3:ac07a11318f9 86 sensor2 = sensor[2];
bayagich 3:ac07a11318f9 87
bayagich 3:ac07a11318f9 88 //high pass equation
bayagich 3:ac07a11318f9 89 altsens = (1 - t*angf)*prevaltsens + sensor2 - prevsensor;
bayagich 3:ac07a11318f9 90
bayagich 3:ac07a11318f9 91 //sine movement
bayagich 3:ac07a11318f9 92 angf = a*angf*cos(angf*t) + c*altsens*sin(angf*t) - d*(altsens*altsens)*sin(angf*t);
bayagich 3:ac07a11318f9 93
bayagich 3:ac07a11318f9 94 u = cos(angf*t);
bayagich 3:ac07a11318f9 95
bayagich 3:ac07a11318f9 96 leftspeed = leftspeed + u;
bayagich 3:ac07a11318f9 97 rightspeed = rightspeed - u;
bayagich 3:ac07a11318f9 98
bayagich 3:ac07a11318f9 99 }//while(!found)
bayagich 2:acc52907bcce 100
mmpeter 0:386c250325ce 101
mmpeter 0:386c250325ce 102
mmpeter 0:386c250325ce 103 }
mmpeter 0:386c250325ce 104
mmpeter 0:386c250325ce 105