Module 9 Super Team / InBetweenController

Changed some stuff

inbetweencontroller.cpp@14:1428f0ba1315, 2015-11-03 (annotated)

Committer:
AeroKev
Date:
Tue Nov 03 11:59:20 2015 +0000
Revision:
14:1428f0ba1315
Parent:
13:ae8af9693111 
I lied

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AeroKev 4:4bfcf0ef3493 1 #include "mbed.h"
AeroKev 0:2166f5140f4f 2 #include "compute.h"
AeroKev 0:2166f5140f4f 3 #include "inbetweencontroller.h"
AeroKev 12:6dbed55bcf43 4 #include <algorithm>
AeroKev 0:2166f5140f4f 5
AeroKev 13:ae8af9693111 6 double x_start = 0;
AeroKev 13:ae8af9693111 7 double y_start = 18;
AeroKev 14:1428f0ba1315 8 double prev_dir;
AeroKev 13:ae8af9693111 9 bool was_pushing = false;
AeroKev 13:ae8af9693111 10 bool goingBack = false;
AeroKev 0:2166f5140f4f 11
AeroKev 2:9482ede4b815 12 void IBC_init(double& ba, double& bb) {
AeroKev 4:4bfcf0ef3493 13 Point2Angles(x_start,y_start,ba,bb);
AeroKev 0:2166f5140f4f 14 }
AeroKev 0:2166f5140f4f 15
dolcaer 7:d7cf5ed50875 16 void newPos(double emg1, double emg2, double emg3, double rad_a, double rad_b, double& nexta, double& nextb, bool& pushing) {
AeroKev 0:2166f5140f4f 17 double next_a, next_b;
dolcaer 7:d7cf5ed50875 18 double dir = emg1-emg2;
AeroKev 14:1428f0ba1315 19 double cur_x, cur_y;
dolcaer 7:d7cf5ed50875 20
AeroKev 3:1541f25bc8d6 21 Angles2Point(rad_a, rad_b, cur_x, cur_y);
dolcaer 7:d7cf5ed50875 22
dolcaer 7:d7cf5ed50875 23 double next_x, next_y;
AeroKev 9:fdb66e7bc47a 24 if(emg3 > 0.5 && !goingBack) {
dolcaer 7:d7cf5ed50875 25 next_x = cur_x;
AeroKev 12:6dbed55bcf43 26 next_y = compute_max('Y', cur_x);
dolcaer 7:d7cf5ed50875 27 was_pushing = true;
dolcaer 7:d7cf5ed50875 28 pushing = true;
AeroKev 14:1428f0ba1315 29 }
AeroKev 14:1428f0ba1315 30 else {
AeroKev 9:fdb66e7bc47a 31 if(was_pushing) {
AeroKev 9:fdb66e7bc47a 32 goingBack = true;
AeroKev 9:fdb66e7bc47a 33 Point2Angles(x_start, y_start, nexta, nextb);
AeroKev 9:fdb66e7bc47a 34 pushing = false;
AeroKev 14:1428f0ba1315 35
AeroKev 14:1428f0ba1315 36 // If the current x and y are close nough to the starting position, stop going back and return control to the player
AeroKev 14:1428f0ba1315 37 if(fabs(cur_x-x_start) < 5 && fabs(cur_y-y_start) < 5) {
AeroKev 9:fdb66e7bc47a 38 was_pushing = false;
AeroKev 9:fdb66e7bc47a 39 goingBack = false;
AeroKev 9:fdb66e7bc47a 40 }
AeroKev 9:fdb66e7bc47a 41 return;
AeroKev 9:fdb66e7bc47a 42 }
dolcaer 7:d7cf5ed50875 43
dolcaer 7:d7cf5ed50875 44 next_y = y_start;
AeroKev 6:7d8b49d91e6b 45 if(dir == 0) next_x = cur_x;
AeroKev 14:1428f0ba1315 46
AeroKev 14:1428f0ba1315 47 // If the robot is moving, move it to one of the waypoints
dolcaer 7:d7cf5ed50875 48 else if(dir > 0) {
AeroKev 9:fdb66e7bc47a 49 if (cur_x >= 15) {
AeroKev 12:6dbed55bcf43 50 next_x = compute_max('X', next_y) - 8;
AeroKev 9:fdb66e7bc47a 51 } else if (cur_x >= 6) {
AeroKev 9:fdb66e7bc47a 52 next_x = 18;
AeroKev 9:fdb66e7bc47a 53 } else if (cur_x >= -3) {
AeroKev 9:fdb66e7bc47a 54 next_x = 9;
AeroKev 9:fdb66e7bc47a 55 } else if (cur_x >= -12) {
AeroKev 9:fdb66e7bc47a 56 next_x = 0;
AeroKev 9:fdb66e7bc47a 57 } else if (cur_x >= -21) {
AeroKev 9:fdb66e7bc47a 58 next_x = -9;
dolcaer 7:d7cf5ed50875 59 } else {
AeroKev 9:fdb66e7bc47a 60 next_x = -18;
dolcaer 7:d7cf5ed50875 61 }
AeroKev 14:1428f0ba1315 62 }
AeroKev 14:1428f0ba1315 63 else {
AeroKev 9:fdb66e7bc47a 64 if (cur_x <= -15)
AeroKev 12:6dbed55bcf43 65 next_x = -compute_max('X', next_y) + 8;
AeroKev 9:fdb66e7bc47a 66 else if (cur_x <= -6) {
AeroKev 9:fdb66e7bc47a 67 next_x = -18;
AeroKev 9:fdb66e7bc47a 68 } else if (cur_x <= 3) {
AeroKev 9:fdb66e7bc47a 69 next_x = -9;
AeroKev 9:fdb66e7bc47a 70 } else if (cur_x <= 12) {
AeroKev 9:fdb66e7bc47a 71 next_x = 0;
AeroKev 9:fdb66e7bc47a 72 } else if (cur_x <= 21) {
AeroKev 9:fdb66e7bc47a 73 next_x = 9;
dolcaer 7:d7cf5ed50875 74 } else {
AeroKev 9:fdb66e7bc47a 75 next_x = 18;
dolcaer 7:d7cf5ed50875 76 }
dolcaer 7:d7cf5ed50875 77 }
dolcaer 7:d7cf5ed50875 78 was_pushing = false;
dolcaer 7:d7cf5ed50875 79 pushing = false;
AeroKev 0:2166f5140f4f 80 }
dolcaer 7:d7cf5ed50875 81
dolcaer 7:d7cf5ed50875 82 prev_dir = dir;
AeroKev 14:1428f0ba1315 83
AeroKev 14:1428f0ba1315 84 // Calculate the angle the motors need to be in in order to get to the desired point.
dolcaer 7:d7cf5ed50875 85 Point2Angles(next_x, next_y, next_a, next_b);
AeroKev 0:2166f5140f4f 86 nexta = next_a;
AeroKev 0:2166f5140f4f 87 nextb = next_b;
AeroKev 6:7d8b49d91e6b 88 }