File content as of revision 4:4bfcf0ef3493:

#include "mbed.h"
#include "compute.h" 
#include "inbetweencontroller.h"
#include <stdlib.h>
#include <stdio.h>

using namespace std;

Serial      pc3(USBTX, USBRX);

float emg1, emg2, emg3; // Right/left
float x_start = 0;
float y_start = 18;
double begin_a, begin_b;

void IBC_init(double& ba, double& bb) {
   Point2Angles(x_start,y_start,ba,bb);
}

void newPos(float emg1, float emg2, float emg3, float rad_a, float rad_b, double& nexta, double& nextb) {
    double next_a, next_b;
    float cur_x, cur_y;
    Angles2Point(rad_a, rad_b, cur_x, cur_y);
    float dir = emg1-emg2;
    if(emg3>0.5) {
        Point2Angles(cur_x, max('Y', cur_x), next_a, next_b);
    } else { 
        double next_x = cur_x+(10*dir);
        double max_x = max('X', cur_y)-3;
        double min_x = -max_x+3;
        if(next_x >= 0) next_x = (next_x > max_x ? max_x : next_x); 
        else next_x = (next_x < min_x ? min_x : next_x); 
        Point2Angles(next_x, cur_y, next_a, next_b);
        pc3.printf("Next_x: %f\r\nMax X: %f\r\nMin X: %f",next_x,max_x,min_x);   
    }
    nexta = next_a;
    nextb = next_b;
}

// int main() {
//  init();
//  double rad_a = deg2rad(192);
//  double rad_b = deg2rad(36);
//  double emg1 = 5;
//  double emg2 = 0.0;
//  double emg3 = 0.3;
//  double next_a, next_b;
//  double cur_a, cur_b;
//  newPos(emg1, emg2, emg3, rad_a, rad_b, next_a, next_b);
//     printf("next_a, next_b = %f, %f", next_a*(180/3.14), next_b*(180/3.14));
//  return 0;
// }