File content as of revision 28:d265c64d2bca:

#include "mbed.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "QEI.h"

DigitalOut led_g(LED_GREEN);
DigitalOut led_b(LED_BLUE);
DigitalOut led_r(LED_RED);

DigitalOut M1_Rotate(D2); // voltage only base rotation
PwmOut M1_Speed(D3);      // voltage only base rotation

MODSERIAL pc(USBTX, USBRX);

//QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING)
QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING);
QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING);

DigitalOut M2_Rotate(D4);   // encoder side pot 2 translation
PwmOut M2_Speed(D5);        // encoder side pot 2 translation

DigitalOut M3_Rotate(D7);   // encoder side pot 1 spatel rotation
PwmOut M3_Speed(D6);        // encoder side pot 1 spatel rotation

DigitalIn links(SW3);
DigitalIn rechts(SW2);

AnalogIn pot1(A4); // pot 1 motor 1
AnalogIn pot2(A3); // pot 2 motor 3

//Define objects
AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );
DigitalIn   buttonCalibrate(D9);

bool draairechts;
bool draailinks;
bool turn = 0;
float waiter = 0.1;
float translation = 0;
float degrees3 = 0;

float Puls_degree = (8400/360);
float wheel1 = 16;
float wheel2 = 31;
float wheel3 = 41;
float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
float pi = 3.14159265359;

volatile float x;
volatile float x_prev =0;
volatile float b; // filtered 'output' of ReadAnalogInAndFilter

bool calibrate = false;
double threshold_Left = 0;
double threshold_Right= 0;
Ticker      sample_timer;
Ticker      sample_timer2;
Ticker      printinfo;
HIDScope    scope( 2 );
DigitalOut  led(LED1);
const double a1 = -1.6475;
const double a2 = 0.7009;
const double b0 = 0.8371;
const double b1 = -1.6742;
const double b2 = 0.8371;
const double c1 = -1.9645;
const double c2 = 0.9651;
const double d0 = 0.0001551;
const double d1 = 0.0003103;
const double d2 = 0.0001551;
double v1_high = 0;
double v2_high = 0;
double v1_low = 0;
double v2_low = 0;
double highpassFilterLeft = 0;
double lowpassFilterLeft = 0;
double highpassFilterRight = 0;
double lowpassFilterRight = 0;

//setpoints
const double Setpoint_Translation = -200;
const double Setpoint_Back = 0;
const double Setpoint_Rotation = pi;
double M3_ControlSpeed = 0;
double M2_ControlSpeed = 0;
double SetpointError_Translation = 0;
double SetpointError_Rotation = 0;

//booleans for control
bool booltranslate = false;
bool boolrotate = false;
//copied from slides
//Arm PID
const double Ts = 0.001953125; //Ts=1/fs (sample frequency)
const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4;
double Translation_error = 0;
double Translation_e_prev = 0;

//Spatel PID
const double Rotation_Kp = 0.0499, Rotation_Ki = 0.0429 , Rotation_Kd = 0.0429;
double Rotation_error = 0;
double Rotation_e_prev = 0;

double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev)
{
    double e_der = (error - e_prev) / Ts;
    e_prev = error;
    e_int = e_int + (Ts * error);

    return kp*error + ki + e_int + kd + e_der;
}

double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
               const double b1, const double b2)
{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1;
    v1 = v;
    return y;
}

/** Sample function
 * this function samples the emg and sends it to HIDScope
 **/

void filterSampleLeft()
{
    highpassFilterLeft = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
    lowpassFilterLeft = biquad1(highpassFilterLeft, v1_low, v2_low, c1, c2, d0, d1, d2);
    scope.set(0, lowpassFilterLeft );
    scope.send();
    //pc.printf("%f \n \r ", lowpassFilter);
}
void filterSampleRight()
{
    highpassFilterRight = fabs(biquad1(emg1.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
    lowpassFilterRight = biquad1(highpassFilterRight, v1_low, v2_low, c1, c2, d0, d1, d2);
    scope.set(1, lowpassFilterRight );
    scope.send();
    //pc.printf("%f \n \r ", lowpassFilter);
}

void sample()
{
    // Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
    scope.set(0, emg0.read() );
    scope.set(1, emg1.read() );
    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
    *  Ensure that enough channels are available (HIDScope scope( 2 ))
    *  Finally, send all channels to the PC at once */

    x = emg0;   // Capture data        scope.set(0, x);   // store data in first element of scope memory
    b = (x_prev + x)/2.0;   // averaging filter
    x_prev = x; // Prepare for next round

    scope.send();
    // To indicate that the function is working, the LED is toggled
    led = !led;
    pc.printf("%f, %f \n \r ", x, b);
}

void GetDirections()
{
    pc.baud(115200);
    if ((rechts == 0) && (links == 0) && (turn == 0)) {
        draailinks = 0;
        draairechts = 0;
        turn = 1;
        pc.printf("begin de actie \n \r ");
        wait(waiter);

    } else if ((rechts == 0) && (links == 0) && (turn == 1)) {
        draailinks = 0;
        draairechts = 0;
        turn = 0;
        pc.printf("breek de actie af \n \r ");
        wait(waiter);
    } else if ((rechts == 1) && (links == 1)&& (turn == 0)) {

    } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
        /* if the right button is pressed and the motor isn't rotating to the left,
        then start rotating to the right etc*/
        draairechts = !draairechts;
        pc.printf("draai naar rechts \n \r ");
        wait(waiter);
    } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
        draailinks = 0;
        draairechts = !draairechts;
        pc.printf("draai naar rechts na links \n \r ");
        wait(waiter);
    } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
        draailinks = !draailinks;
        pc.printf("draai naar links \n \r ");
        wait(waiter);
    } else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
        draairechts = 0;
        draailinks = !draailinks;
        pc.printf("draai naar links na rechts \n \r ");
        wait(waiter);
    }
    wait(2*waiter);
}

float GetPositionM2()
{
    float pulses2 = motor2.getPulses();
    float degrees2 = (pulses2/Puls_degree);
    float radians2 = (degrees2/360)*2*pi;
    float translation = ((radians2/overbrenging)*32.25);

    return translation;
}
float GetRotationM3()
{
    float pulses3 = motor3.getPulses();
    float degrees3 = (pulses3/Puls_degree);
    float radians3 = (degrees3/360)*2*pi;

    return radians3;
}
void motorRotation(double setpoint)
{
    double theta_rotation = GetRotationM3();
    SetpointError_Rotation =  setpoint - theta_rotation;

    //set direction
    if (SetpointError_Rotation > 0) {
        M3_Rotate = 0;
    } else {
        M3_Rotate = 1;

    }
    M3_ControlSpeed = Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
    if (fabs(SetpointError_Rotation) < fabs(Setpoint_Rotation*0.05)) {
        M3_ControlSpeed = 0;
    }
    if (theta_rotation > (Setpoint_Rotation*0.9))
        boolrotate = true;
    if ((theta_rotation < (Setpoint_Rotation*0.07) ) && (M3_Speed == 0))
        boolrotate = false;
        M3_Speed = M3_ControlSpeed;
}
void motorTranslation(double setpoint)
{
    double theta_translation = GetPositionM2();
    SetpointError_Translation =  setpoint - theta_translation;

    //set direction
    if (SetpointError_Translation < 0) {
        M2_Rotate = 0;
    } else {
        M2_Rotate = 1;
    }
    M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
    if (fabs(SetpointError_Translation) < fabs(Setpoint_Translation*0.05)) {
        M2_ControlSpeed = 0;
        
    }
    if ((theta_translation < Setpoint_Translation*0.95) && (M2_ControlSpeed == 0))
        booltranslate = true;
    if ((theta_translation > Setpoint_Translation*0.05) && (M2_ControlSpeed == 0))
        booltranslate = false;
    M2_Speed = M2_ControlSpeed;
  
}
void GoBack()
{
    motorTranslation(Setpoint_Back);
    if (booltranslate == false) {
        motorRotation(Setpoint_Back);
    }
    if (boolrotate == false) {
        turn = 0;
    }
    led_r = 1;
    led_b = 0;

    /* while (GetPositionM2() < 0) {
         M3_Speed = 0;
         M2_Speed = 1;
         M2_Rotate = 1;
         pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
         led_r = 0;
     }
     M2_Speed = 0;


     while (GetRotationM3() > 0) {
         M3_Rotate = 1;
         M3_Speed = 0.2;
         led_r = 1;
         led_b = 0;
         pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());

     }
     M3_Speed = 0;

     turn = 0; */
}

void Burgerflip()
{
    led_r = 0;
    led_b = 1;
    motorTranslation(Setpoint_Translation);
    if (booltranslate == true) {
        motorRotation(Setpoint_Rotation);
    }
    /*
    pc.printf("get position %f, get rotation %f \n \r", GetPositionM2(), GetRotationM3());
    if (GetPositionM2()< afstand) {
        M3_Speed = 0.2;
        M3_Rotate = 0;
        M2_Speed = 0;
    } else if (GetPositionM2() > afstand) {
        M2_Speed = 1;
        M2_Rotate = 0;

    }
    if (GetRotationM3() > setpoint_Rotation) {
        GoBack();
    }
    */
}
void BurgerflipActie()
{
    Burgerflip();
    if (boolrotate == true) {
        GoBack();
    }
}
void print()
{
    pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
}
int main()
{
    //Leds
    led_g = 1;
    led_b = 1;
    led_r = 1;

    /**Attach the 'sample' function to the timer 'sample_timer'.
    * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
    */
    //sample_timer.attach(&sample, 0.001953125);
    sample_timer2.attach(&filterSampleLeft, Ts);        //512 Hz
    sample_timer.attach(&filterSampleRight, Ts);
    //printinfo.attach(&print, Ts);
    pc.baud(115200);
    pc.printf("please push the button to calibrate \n \r");
    while (1) {
        if (buttonCalibrate == 0) {
            calibrate = true;
            threshold_Left = lowpassFilterLeft*0.7;
            threshold_Right = lowpassFilterRight*0.7;

        }
        if (calibrate == true) {
            //pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right);
            pc.printf("rotation is %f error = %f en translation = %f en de error %f \n \r", GetRotationM3(), SetpointError_Rotation, GetPositionM2(), SetpointError_Translation);
            GetDirections();
            if (draairechts == true) {
                M1_Speed = 0.2;
                M1_Rotate = 0;
            } else if (draailinks == true) {
                M1_Speed = 0.2;
                M1_Rotate = 1;
            } else if (turn == 1) {
                /*M2_Speed = 0.5;
                M2_Rotate = 1;
                M3_Speed = 0.5;
                M3_Rotate = 1;*/
                BurgerflipActie();
            } else if (turn == 0) {
                M2_Speed = 0;
                M3_Speed = 0;
            }
            if ((draailinks == false) && (draairechts == false)) {
                M1_Speed = 0;
            }
            //pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
            /* pulses = 8400 */
            /*empty loop, sample() is executed periodically*/
        }
    }
}