der Roboter / Mbed 2 deprecated VariablesGradient

Explanation of Variables

Dependencies:   m3pi_ng mbed

Fork of BlueToothStuff by der Roboter

main.cpp@6:ed2f00b16b95, 2014-06-12 (annotated)

Committer:
bayagich
Date:
Thu Jun 12 11:18:05 2014 +0000
Revision:
6:ed2f00b16b95
Parent:
5:c91ef337eff4 
Playing around with values, but everything should be working

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"
bayagich 6:ed2f00b16b95 11 #include <cmath>
mmpeter 0:386c250325ce 12 #include "iostream"
satyson 1:6402638c6f6d 13 #include "btbee.h"
bayagich 6:ed2f00b16b95 14 #include "Timer.h"
bayagich 6:ed2f00b16b95 15 #include <stdlib.h>
mmpeter 0:386c250325ce 16
mmpeter 0:386c250325ce 17 using namespace std;
mmpeter 0:386c250325ce 18
mmpeter 0:386c250325ce 19 m3pi thinggy;
satyson 1:6402638c6f6d 20 btbee btbee;
bayagich 6:ed2f00b16b95 21 Timer mytime;
bayagich 6:ed2f00b16b95 22
bayagich 6:ed2f00b16b95 23 DigitalIn button(p21);
mmpeter 0:386c250325ce 24
bayagich 6:ed2f00b16b95 25 float PI=3.141592;
mmpeter 0:386c250325ce 26 int main() {
bayagich 2:acc52907bcce 27
bayagich 3:ac07a11318f9 28 int sensor[5];
bayagich 3:ac07a11318f9 29
bayagich 6:ed2f00b16b95 30 //w = angular frequency, which affects how much the robot oscilates
bayagich 6:ed2f00b16b95 31 float angf = 2*PI;
bayagich 2:acc52907bcce 32
bayagich 6:ed2f00b16b95 33 //variables in the sine equation to calculate u
bayagich 6:ed2f00b16b95 34 double a = .01;
bayagich 6:ed2f00b16b95 35 double c = .0007;
bayagich 6:ed2f00b16b95 36 double d = .00000001;
bayagich 2:acc52907bcce 37
bayagich 3:ac07a11318f9 38 //the middle sensor value you take every t seconds
bayagich 3:ac07a11318f9 39 double sensor2;
bayagich 3:ac07a11318f9 40 //you need the previous sensor for the high pass equation
bayagich 5:c91ef337eff4 41 double prevsensor = 0;
bayagich 2:acc52907bcce 42
bayagich 3:ac07a11318f9 43 //this u alters left speed and right speed
bayagich 3:ac07a11318f9 44 double u;
bayagich 6:ed2f00b16b95 45 double fwdspeed = 0.1;
bayagich 3:ac07a11318f9 46 float leftspeed = .01;
bayagich 3:ac07a11318f9 47 float rightspeed = .01;
bayagich 2:acc52907bcce 48
bayagich 3:ac07a11318f9 49 //when you pass the sensor values through high pass
bayagich 3:ac07a11318f9 50 //altered sensor
bayagich 3:ac07a11318f9 51 double altsens;
bayagich 3:ac07a11318f9 52 //previous altered sensor
bayagich 3:ac07a11318f9 53 double prevaltsens = 0;
mmpeter 0:386c250325ce 54
bayagich 3:ac07a11318f9 55 //read in sensor value
bayagich 6:ed2f00b16b95 56 thinggy.raw_sensor(sensor);
bayagich 3:ac07a11318f9 57 sensor2 = sensor[2];
bayagich 2:acc52907bcce 58
bayagich 5:c91ef337eff4 59 //how to time
bayagich 6:ed2f00b16b95 60 double t_actual;
bayagich 6:ed2f00b16b95 61 mytime.start();
bayagich 5:c91ef337eff4 62
bayagich 6:ed2f00b16b95 63 //how to increment through
bayagich 6:ed2f00b16b95 64 double cur_step = 0;
bayagich 6:ed2f00b16b95 65 double max_steps = 100; //3 should run for a minute
bayagich 6:ed2f00b16b95 66
bayagich 6:ed2f00b16b95 67 //how frequently the robot takes times
bayagich 6:ed2f00b16b95 68 double delta_t = 0.01;
mmpeter 0:386c250325ce 69
bayagich 6:ed2f00b16b95 70 //loop to change w as it picks up values sensor[2] != 2000
bayagich 6:ed2f00b16b95 71 while(cur_step < max_steps){
bayagich 5:c91ef337eff4 72 //robot will go too fast if it is over .7, so this caps the speed at .7
bayagich 5:c91ef337eff4 73 if(leftspeed >= .7 and rightspeed >= .7){
bayagich 5:c91ef337eff4 74 thinggy.right_motor(.7);
bayagich 5:c91ef337eff4 75 thinggy.left_motor(.7);
bayagich 4:db3011440b4f 76 }
bayagich 5:c91ef337eff4 77 else if(leftspeed >= .7){
bayagich 5:c91ef337eff4 78 thinggy.left_motor(.7);
bayagich 4:db3011440b4f 79 thinggy.right_motor(rightspeed);
bayagich 4:db3011440b4f 80 }
bayagich 5:c91ef337eff4 81 else if (rightspeed >= .7){
bayagich 5:c91ef337eff4 82 thinggy.right_motor(.7);
bayagich 4:db3011440b4f 83 thinggy.left_motor(leftspeed);
bayagich 4:db3011440b4f 84 }
bayagich 4:db3011440b4f 85 else {
bayagich 4:db3011440b4f 86 thinggy.right_motor(leftspeed);
bayagich 4:db3011440b4f 87 thinggy.left_motor(rightspeed);
bayagich 4:db3011440b4f 88 }
bayagich 5:c91ef337eff4 89
bayagich 6:ed2f00b16b95 90 t_actual = mytime.read();
bayagich 6:ed2f00b16b95 91
bayagich 6:ed2f00b16b95 92 thinggy.raw_sensor(sensor);
bayagich 6:ed2f00b16b95 93 sensor2=sensor[2];
bayagich 3:ac07a11318f9 94
bayagich 6:ed2f00b16b95 95 //high pass equation maybe needs to be made positive?
bayagich 6:ed2f00b16b95 96 altsens = -1*abs((1 - (.05*2*PI)*delta_t)*prevaltsens + sensor2 - prevsensor);
bayagich 5:c91ef337eff4 97
bayagich 3:ac07a11318f9 98 //sine movement
bayagich 5:c91ef337eff4 99 u = a*angf*cos(angf*t_actual) + c*altsens*sin(angf*t_actual) - d*(altsens*altsens)*sin(angf*t_actual);
bayagich 6:ed2f00b16b95 100 //d is a variable to show the maximum
bayagich 6:ed2f00b16b95 101
bayagich 6:ed2f00b16b95 102 thinggy.cls();
bayagich 6:ed2f00b16b95 103 thinggy.locate(0,0);
bayagich 6:ed2f00b16b95 104 thinggy.printf(" s:%f", sensor2);
bayagich 6:ed2f00b16b95 105 thinggy.locate(0,1);
bayagich 6:ed2f00b16b95 106 thinggy.printf("u:%f",u);
bayagich 6:ed2f00b16b95 107 leftspeed = fwdspeed + u;
bayagich 6:ed2f00b16b95 108 rightspeed = fwdspeed - u;
bayagich 6:ed2f00b16b95 109
bayagich 6:ed2f00b16b95 110 prevaltsens=altsens;
bayagich 6:ed2f00b16b95 111 prevsensor = sensor2;
bayagich 6:ed2f00b16b95 112
bayagich 6:ed2f00b16b95 113 //make it run through delta t before it collects next sample
bayagich 6:ed2f00b16b95 114 while(t_actual < cur_step + delta_t){
bayagich 6:ed2f00b16b95 115 t_actual = mytime.read();
bayagich 6:ed2f00b16b95 116 }
bayagich 6:ed2f00b16b95 117 cur_step = cur_step + delta_t;
bayagich 6:ed2f00b16b95 118
bayagich 6:ed2f00b16b95 119 }//while
bayagich 6:ed2f00b16b95 120
bayagich 6:ed2f00b16b95 121 thinggy.stop();
bayagich 6:ed2f00b16b95 122 wait(500);
mmpeter 0:386c250325ce 123 }
mmpeter 0:386c250325ce 124
mmpeter 0:386c250325ce 125