Module 9 Super Team / InBetweenController

Changed some stuff

inbetweencontroller.cpp@1:1e855978b9a2, 2015-10-20 (annotated)

Committer:
AeroKev
Date:
Tue Oct 20 11:38:17 2015 +0000
Revision:
1:1e855978b9a2
Parent:
0:2166f5140f4f
Child:
2:9482ede4b815
fucking hell

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AeroKev 0:2166f5140f4f 1 #include "compute.h"
AeroKev 0:2166f5140f4f 2 #include "inbetweencontroller.h"
AeroKev 0:2166f5140f4f 3 #include <stdlib.h>
AeroKev 0:2166f5140f4f 4 #include <stdio.h>
AeroKev 0:2166f5140f4f 5
AeroKev 0:2166f5140f4f 6 using namespace std;
AeroKev 0:2166f5140f4f 7
AeroKev 0:2166f5140f4f 8 float emg1, emg2, emg3; // Right/left
AeroKev 0:2166f5140f4f 9 float x_start = 0;
AeroKev 0:2166f5140f4f 10 float y_start = 15;
AeroKev 0:2166f5140f4f 11 double begin_a, begin_b;
AeroKev 0:2166f5140f4f 12
AeroKev 1:1e855978b9a2 13 void IBC_init(double a, double b) {
AeroKev 1:1e855978b9a2 14 begin_a = a;
AeroKev 1:1e855978b9a2 15 begin_b = b;
AeroKev 0:2166f5140f4f 16 }
AeroKev 0:2166f5140f4f 17
AeroKev 0:2166f5140f4f 18 void newPos(float emg1, float emg2, float emg3, float rad_a, float rad_b, double& nexta, double& nextb) {
AeroKev 0:2166f5140f4f 19 double next_a, next_b;
AeroKev 0:2166f5140f4f 20 float cur_x, cur_y;
AeroKev 0:2166f5140f4f 21 Angles2Point(rad2deg(rad_a), rad2deg(rad_b), cur_x, cur_y);
AeroKev 0:2166f5140f4f 22 float dir = emg1-emg2;
AeroKev 0:2166f5140f4f 23 if(emg3>0.5) {
AeroKev 0:2166f5140f4f 24 Point2Angles(cur_x, max('Y', cur_x), next_a, next_b);
AeroKev 0:2166f5140f4f 25 } else {
AeroKev 0:2166f5140f4f 26 double next_x = cur_x+(10*dir);
AeroKev 0:2166f5140f4f 27 double max_x = max('X', cur_y);
AeroKev 0:2166f5140f4f 28 double min_x = -max_x;
AeroKev 0:2166f5140f4f 29 if(next_x >= 0) next_x = (next_x > max_x ? max_x : next_x);
AeroKev 0:2166f5140f4f 30 else next_x = (next_x < min_x ? min_x : next_x);
AeroKev 0:2166f5140f4f 31 Point2Angles(next_x, cur_y, next_a, next_b);
AeroKev 0:2166f5140f4f 32 }
AeroKev 0:2166f5140f4f 33 nexta = next_a;
AeroKev 0:2166f5140f4f 34 nextb = next_b;
AeroKev 0:2166f5140f4f 35 }
AeroKev 0:2166f5140f4f 36
AeroKev 0:2166f5140f4f 37 // int main() {
AeroKev 0:2166f5140f4f 38 // init();
AeroKev 0:2166f5140f4f 39 // double rad_a = deg2rad(192);
AeroKev 0:2166f5140f4f 40 // double rad_b = deg2rad(36);
AeroKev 0:2166f5140f4f 41 // double emg1 = 5;
AeroKev 0:2166f5140f4f 42 // double emg2 = 0.0;
AeroKev 0:2166f5140f4f 43 // double emg3 = 0.3;
AeroKev 0:2166f5140f4f 44 // double next_a, next_b;
AeroKev 0:2166f5140f4f 45 // double cur_a, cur_b;
AeroKev 0:2166f5140f4f 46 // newPos(emg1, emg2, emg3, rad_a, rad_b, next_a, next_b);
AeroKev 0:2166f5140f4f 47 // printf("next_a, next_b = %f, %f", next_a*(180/3.14), next_b*(180/3.14));
AeroKev 0:2166f5140f4f 48 // return 0;
AeroKev 0:2166f5140f4f 49 // }