der Roboter / Mbed 2 deprecated VariablesGradient

Explanation of Variables

Dependencies:   m3pi_ng mbed

Fork of BlueToothStuff by der Roboter

main.cpp@5:c91ef337eff4, 2014-06-10 (annotated)

Committer:
bayagich
Date:
Tue Jun 10 12:55:20 2014 +0000
Revision:
5:c91ef337eff4
Parent:
4:db3011440b4f
Child:
6:ed2f00b16b95
very slow circles

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"
bayagich 5:c91ef337eff4 14 #include <ctime>
mmpeter 0:386c250325ce 15
mmpeter 0:386c250325ce 16 using namespace std;
mmpeter 0:386c250325ce 17
mmpeter 0:386c250325ce 18 m3pi thinggy;
satyson 1:6402638c6f6d 19 btbee btbee;
mmpeter 0:386c250325ce 20
mmpeter 0:386c250325ce 21 int main() {
bayagich 2:acc52907bcce 22
bayagich 3:ac07a11318f9 23 int sensor[5];
bayagich 3:ac07a11318f9 24
bayagich 3:ac07a11318f9 25 //w = angular frequency, which is modified after you take each time
bayagich 5:c91ef337eff4 26 double angf = 0.01;
bayagich 2:acc52907bcce 27
bayagich 3:ac07a11318f9 28 //variables in the equation to calculate angular frequency
bayagich 5:c91ef337eff4 29 double a = .001;
bayagich 5:c91ef337eff4 30 double c = .001;
bayagich 5:c91ef337eff4 31 double d = .001;
bayagich 2:acc52907bcce 32
bayagich 3:ac07a11318f9 33 //the middle sensor value you take every t seconds
bayagich 3:ac07a11318f9 34 double sensor2;
bayagich 3:ac07a11318f9 35 //you need the previous sensor for the high pass equation
bayagich 5:c91ef337eff4 36 double prevsensor = 0;
bayagich 2:acc52907bcce 37
bayagich 3:ac07a11318f9 38 //this u alters left speed and right speed
bayagich 3:ac07a11318f9 39 double u;
bayagich 3:ac07a11318f9 40 float leftspeed = .01;
bayagich 3:ac07a11318f9 41 float rightspeed = .01;
bayagich 2:acc52907bcce 42
bayagich 3:ac07a11318f9 43 //when you pass the sensor values through high pass
bayagich 3:ac07a11318f9 44 //altered sensor
bayagich 3:ac07a11318f9 45 double altsens;
bayagich 3:ac07a11318f9 46 //previous altered sensor
bayagich 3:ac07a11318f9 47 double prevaltsens = 0;
mmpeter 0:386c250325ce 48
bayagich 4:db3011440b4f 49 thinggy.sensor_auto_calibrate();
bayagich 4:db3011440b4f 50
bayagich 3:ac07a11318f9 51 //read in sensor value
bayagich 4:db3011440b4f 52 thinggy.calibrated_sensor(sensor);
bayagich 3:ac07a11318f9 53 sensor2 = sensor[2];
bayagich 2:acc52907bcce 54
bayagich 5:c91ef337eff4 55 //how to time
bayagich 5:c91ef337eff4 56 //time_t start_t;
bayagich 5:c91ef337eff4 57 //time_t end_t;
bayagich 5:c91ef337eff4 58 double t_actual = .01;
bayagich 5:c91ef337eff4 59 //double t_change = .01;
bayagich 5:c91ef337eff4 60 double t_set = 0.01;
bayagich 5:c91ef337eff4 61
bayagich 3:ac07a11318f9 62 //variable if the best point is found
bayagich 5:c91ef337eff4 63 //int found = 0;
mmpeter 0:386c250325ce 64
bayagich 3:ac07a11318f9 65 //loop to change w as it picks up values
bayagich 5:c91ef337eff4 66 while(1){
bayagich 5:c91ef337eff4 67 //time(&start_t);
bayagich 2:acc52907bcce 68
bayagich 5:c91ef337eff4 69 //robot will go too fast if it is over .7, so this caps the speed at .7
bayagich 5:c91ef337eff4 70 if(leftspeed >= .7 and rightspeed >= .7){
bayagich 5:c91ef337eff4 71 thinggy.right_motor(.7);
bayagich 5:c91ef337eff4 72 thinggy.left_motor(.7);
bayagich 4:db3011440b4f 73 }
bayagich 5:c91ef337eff4 74 else if(leftspeed >= .7){
bayagich 5:c91ef337eff4 75 thinggy.left_motor(.7);
bayagich 4:db3011440b4f 76 thinggy.right_motor(rightspeed);
bayagich 4:db3011440b4f 77 }
bayagich 5:c91ef337eff4 78 else if (rightspeed >= .7){
bayagich 5:c91ef337eff4 79 thinggy.right_motor(.7);
bayagich 4:db3011440b4f 80 thinggy.left_motor(leftspeed);
bayagich 4:db3011440b4f 81 }
bayagich 4:db3011440b4f 82 else {
bayagich 4:db3011440b4f 83 thinggy.right_motor(leftspeed);
bayagich 4:db3011440b4f 84 thinggy.left_motor(rightspeed);
bayagich 4:db3011440b4f 85 }
bayagich 5:c91ef337eff4 86
bayagich 5:c91ef337eff4 87 wait(t_actual);
bayagich 3:ac07a11318f9 88
bayagich 3:ac07a11318f9 89 prevsensor = sensor2;
bayagich 5:c91ef337eff4 90 thinggy.calibrated_sensor(sensor);
bayagich 3:ac07a11318f9 91 sensor2 = sensor[2];
bayagich 5:c91ef337eff4 92
bayagich 5:c91ef337eff4 93 //time(&end_t);
bayagich 5:c91ef337eff4 94 //t_change = difftime(start_t, end_t);
bayagich 5:c91ef337eff4 95 //t_actual = t_change;
bayagich 5:c91ef337eff4 96
bayagich 5:c91ef337eff4 97 //makes it wait amount of time leftover
bayagich 5:c91ef337eff4 98 //if(t_change < t_set){
bayagich 5:c91ef337eff4 99 //wait until time == t
bayagich 5:c91ef337eff4 100 // while(t_change < t_set){
bayagich 5:c91ef337eff4 101 // time(&end_t);
bayagich 5:c91ef337eff4 102 // t_change = difftime(start_t, end_t);
bayagich 5:c91ef337eff4 103 //}
bayagich 5:c91ef337eff4 104 //}
bayagich 3:ac07a11318f9 105
bayagich 3:ac07a11318f9 106 //high pass equation
bayagich 5:c91ef337eff4 107 altsens = (1 - t_set*angf)*prevaltsens + sensor2 - prevsensor;
bayagich 5:c91ef337eff4 108
bayagich 3:ac07a11318f9 109 //sine movement
bayagich 5:c91ef337eff4 110 u = a*angf*cos(angf*t_actual) + c*altsens*sin(angf*t_actual) - d*(altsens*altsens)*sin(angf*t_actual);
bayagich 3:ac07a11318f9 111
bayagich 5:c91ef337eff4 112 leftspeed = leftspeed - u;
bayagich 5:c91ef337eff4 113 rightspeed = rightspeed + u;
bayagich 3:ac07a11318f9 114
bayagich 3:ac07a11318f9 115 }//while(!found)
bayagich 2:acc52907bcce 116
mmpeter 0:386c250325ce 117
mmpeter 0:386c250325ce 118
mmpeter 0:386c250325ce 119 }
mmpeter 0:386c250325ce 120
mmpeter 0:386c250325ce 121