File content as of revision 0:2166f5140f4f:

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

using namespace std;

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

void init() {
    Point2Angles(0, 15, begin_a, begin_b);
}

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(rad2deg(rad_a), rad2deg(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);
        double min_x = -max_x;
        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);
    }
    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;
// }