File content as of revision 20:04b5619a4fb7:

#include "mbed.h"
#include "math.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "FastPWM.h"
#include "QEI.h"
#include "BiQuad.h"
#define SERIAL_BAUD 115200  // baud rate for serial communication

// Serial connection
MODSERIAL pc(USBTX,USBRX);

//EMG aansluitingen
AnalogIn    emg0( A0 );     //Biceps Rechts
AnalogIn    emg1( A1 );     //Bicpes Links
AnalogIn    emg2( A2 );     //Upper leg

// Setup Potmeters
// Potmeter 1 gives the reference position x
AnalogIn pot1(A3);
// Potmeter 2 gives the reference position y
AnalogIn pot2(A4);

// Setup Buttons
InterruptIn button1(PTB9);                  // button 1
InterruptIn button2(PTA1);                  // button 2
InterruptIn button3(PTC6);                  // SW2
InterruptIn button4(PTA4);                  // SW3

// Setup botton values
bool button1_value = false;
bool button2_value = false;

// Set motor Pinouts
DigitalOut motor1_dir(D4);
FastPWM motor1_pwm(D5);
DigitalOut motor2_dir(D7);
FastPWM motor2_pwm(D6);

// Set LED pins
DigitalOut led(PTE24);                      // D15 LED1
DigitalOut red(LED_RED);
DigitalOut blue(LED_BLUE);
DigitalOut green(LED_GREEN);

// Set HID scope
HIDScope    scope(6);

// Set encoder
QEI m1_EncoderCW(D10,D11,NC,32);
QEI m1_EncoderCCW(D11,D10,NC,32);
QEI m2_EncoderCW(D13,D12,NC,32);
QEI m2_EncoderCCW(D12,D13,NC,32);

// Constants for control
volatile const int   COUNTS_PER_REV = 4200;
volatile const double DEGREES_PER_PULSE = 8400 / 360;

// Set initial Kp and Ki
volatile double Kp = 0.05; // last known good Kp: 0.02
volatile double Ki = 0.025;   // last known good Ki: 0.0025
volatile double Kd = 0.0; // last known good Kd: 0.0

// Set frequencies for sampling, calc and control.
volatile const double SAMPLE_F = 500;               // 500 Hz
volatile const double SAMPLE_TS = 1.0/SAMPLE_F;
volatile const double CALC_F = 500.0;               // 100 Hz
volatile const double CALC_TS = 1.0/CALC_F;
volatile const double CONTROLLER_TS = 0.01;         // 100 Hz

volatile double Ts = 0.01;
volatile double N = 100;

// Memory values for controllers
double m1_v1 = 0, m1_v2 = 0;
double m2_v1 = 0, m2_v2 = 0;

//BiQuad
// making the biquads and chains; imported from matlab
BiQuadChain bqc;        //Chain for right biceps
BiQuadChain bqc2;       //Chain for left biceps
BiQuadChain bqc3;       //Chain for Upper leg
// 1 to 3 are for right biceps
BiQuad bq1( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
BiQuad bq2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
BiQuad bq3( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
// 4 to 6 are for left biceps
BiQuad bq4( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
BiQuad bq5( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
BiQuad bq6( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
// 7 to 9 are for upper leg
BiQuad bq7( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
BiQuad bq8( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
BiQuad bq9( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );

// Variables for filter
// muscle EMG < treshhold
volatile bool BicepsLeft = false;
volatile bool BicepsRight = false;
volatile bool Leg = false;

// Variables for MotorHoekBerekenen (MotorAngleCalculation)
volatile double x = 0.0;                             // beginpositie x en y
volatile double y = 305.5;
volatile const double pi = 3.14159265359; 
volatile double Speed = 60;                          // Speed with which x and y are changed, in mm/s
volatile double Theta1Gear = 60.0;                   // Beginpositie op 60 graden
volatile double Theta2Gear = 60.0;
volatile const int LargeGear = 42;
volatile const int SmallGear = 15;
volatile double Theta1 = Theta1Gear*LargeGear/SmallGear;           // Beginpositie van MotorHoek
volatile double Theta2 = Theta2Gear*LargeGear/SmallGear;

// Stepper motor setup
// Set constans
volatile int stepmode = 16;                         // step mode
volatile int steps = 150*stepmode;                  // amount of steps for 90 degree rotation = 50, full step mode: 1.8 degree per step.
volatile int i = 0;
volatile bool Stepper_CCW = false;

// Setup stepper motor pins
// DigitalOut pinMode(PTD2);     //D11 NOW USES 3.3V OUPUT, SINCE THIS IS ALWAYS ON
DigitalOut pinSleep(PTE25);      //D14
DigitalOut pinStep(PTC4);        //D9
DigitalOut pinDir(PTC12);        //D8

// Control of the stepper motor with 1 Ticker and 1 Timeout
Ticker  TickerStepper;
Timeout TimeoutStepper;

// Global variable Step_State
volatile bool Stepper_State = false;        // false = we zijn niet bezig met flippen

// Setup Tickers
    Ticker      CalculationsTicker;
    Ticker      BlinkLedTicker;
    Ticker      PIDControlTicker;
    Ticker      PotControlTicker;

/////////////////////// Functtions for stepper motor /////////////////////////////////////
// Set the Step pin to 0
void StepperFall()
{
    pinStep = 0;
}
// Turn off stepper and power off driver board. Reset Stepper_CCS, i and Stepper_State
void Stepper_Off()
{
    TickerStepper.detach();
    pinSleep = 0;
    i=0;
    Stepper_State = false;
    Stepper_CCW = false;
}

// Change direction
void Stepper_Change_Dir()
{
    pinDir = true;
    Stepper_CCW = true;
    i = 0; 
}

// Move the motor 1 step
void StepperRise()
{
    if((i<steps)&&(not Stepper_CCW))
    {
        pinStep=1; // Set stepper to 1
        // attach timeout to set it back to 0, min step duration is 1.8 us
        TimeoutStepper.attach_us(&StepperFall, 2);
        i=i+1;
    }
    else if (Stepper_CCW)
    {
        if(i<steps)
        {
            pinStep=1;
            TimeoutStepper.attach_us(&StepperFall, 2);
            i=i+1;
        }
        else 
        {
            Stepper_Off();
        }
    }
    else
    {
         Stepper_Change_Dir();
    }
}

// Activate the stepper motor
// at a constant speed
void Stepper_On()
{
    if (Stepper_State == false)
    {
    // Set global variable to true
    Stepper_State = true;
    // Power on the board and set direction to CW
    pinSleep = 1;
    pinDir = false;
    // Calc the speed of the stepper
    int nPPS = 50*stepmode;  // was 1200, this controlls the speed, keep this times the step mode
    double fFrequency_Hz = 1.f * nPPS;
    double fPeriod_s = 1.0f / fFrequency_Hz;
    // Check if the speed is too fast small and report back if that is the case
    if (fPeriod_s < 2e-6)
    {
        pc.printf("\r\n ERROR: fPeriod_s too small \r\n");
        fPeriod_s=2e-6;
    }    
    
    TickerStepper.attach(&StepperRise, // void(*fptr)(void)
                        fPeriod_s   // float t
                        );
    }
}

/////////////////////// functions for filters /////////////////////////////////////////////
void sample()
{
    /*  Sample function
    samples the emg0 (of the rightBiceps)
    samples the emg1 (of the leftBiceps)
    samples the emg2 (of the Upper leg)
    filter the singals
    sends it to HIDScope
    */
    
    // Rechts Biceps
    double inRechts  = emg0.read();                 // EMG signal
    double FilterRechts = bqc.step(inRechts);       // High pass + Notch (50 HZ)
    double RectifyRechts = fabs(FilterRechts);      // Rectify
    double outRechts =  bq3.step(RectifyRechts);    // Low pass
   
    // Links Biceps
    double inLinks  = emg1.read();                  // EMG signal
    double FilterLinks = bqc2.step(inLinks);        // High pass + Notch (50 HZ)
    double RectifyLinks = fabs(FilterLinks);        // Rectify
    double outLinks =  bq6.step(RectifyLinks);      // Low pass
    
    // Upper leg
    double inBeen  = emg2.read();                   // EMG signal
    double FilterBeen = bqc3.step(inBeen);          // High pass + Notch (50 HZ)
    double RectifyBeen = fabs(FilterBeen);          // Rectify
    double outBeen =  bq9.step(RectifyBeen);        // Low pass 
    
    
    /* EMG signal in channel 0 (the first channel) 
    and the filtered EMG signal in channel 1 (the second channel)
    of the HIDscope */
    /*scope.set(0,inRechts);
    scope.set(1,outRechts);
    scope.set(2,inLinks);
    scope.set(3,outLinks);
    scope.set(4,inBeen);
    scope.set(5,outBeen);
    */
    // To indicate that the function is working, the LED is on
    if (outRechts >= 0.03){
        BicepsRight = true;
        red = 0;
        }
    else{
        BicepsRight = false;
        red = 1;
    }

    // If Biceps links is actuated then:
    if (outLinks >= 0.02){
        BicepsLeft = true;
        green = 0;
        }
    else{
        BicepsLeft = false;
        green = 1;
    }
     // If upper leg is actuated then:
    if (outBeen >= 0.02){
        Leg = true;
        blue = 0;
        }
    else{
        Leg = false;
        blue = 1;
    }
}

///////////////////// functions for "motorhoekberekenen" //////////////////////////
// save previous x 
double Calc_Prev_x () {
    double Prev_x = x;
        //pc.printf("prev x = %f\r\n", Prev_x);
    return Prev_x;
}

// save previous y
double Calc_Prev_y () {
    double Prev_y = y;
        //pc.printf("prev y = %f\r\n", Prev_y);
    return Prev_y;
}

// save previous Theta1Gear
double Calc_Prev_Theta1_Gear () {
    double Prev_Theta1_Gear = Theta1Gear;
    return Prev_Theta1_Gear;
}

// save previous Theta2Gear
double Calc_Prev_Theta2_Gear () {
    double Prev_Theta2_Gear = Theta2Gear;
    return Prev_Theta2_Gear;
}

void CalcXY () 
{
    // calc steps in mm
    double Step = Speed/CALC_F ;  //10 mm per seconde afleggen
    int Ymax = 306;
    int Xmax = 200;
    if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) {   
        Stepper_On();
    }
    else if (BicepsLeft==true && BicepsRight==false && Leg==false && Stepper_State==false) {
        x = x - Step;
    }
    else if (BicepsLeft==false && BicepsRight==true && Leg==false && Stepper_State==false) {
        x = x + Step;       // naar Right bewegen
    }
    else if (BicepsLeft==true && BicepsRight==true && Leg==false && Stepper_State==false) {
        y = y + Step;        // naar voren bewegen
    }
    else if (BicepsLeft==false && BicepsRight==true && Leg==true && Stepper_State==false) {
        y = y - Step;       // naar achter bewegen
    }
    else if (BicepsLeft==false && BicepsRight==false && Leg==false || Stepper_State==true) {
    }
    
    // Grenswaardes     LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
    if (x > Xmax) {
        x = Xmax;
    }
    else if (x < -Xmax) {
        x = -Xmax;
    }
    if (y > Ymax) {
        y = Ymax;
    }
    else if (y < 50) {
        y = 50;         // GOKJE, UITPROBEREN
    }
    //pc.printf("x = %f, y = %f\r\n", x, y);
      
    //scope.set(2,x);
    //scope.set(3,y);
}    

// diagonaal berekenen voor linker arm
double CalcDia1 (double x, double y) {
    double a = 50.0;                                // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden)   KEERTJE NAMETEN
    double BV1 = sqrt(pow((a+x),2) + pow(y,2));     // diagonaal (afstand van armas tot locatie) berekenen
    double Dia1 = pow(BV1,2)/(2*BV1);               // berekenen van de afstand oorsprong tot diagonaal
    
    //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia1, x, y);
    return Dia1;
}

// diagonaal berekenen voor rechter arm
double CalcDia2 (double x, double y) {
    double a = 50.0;                                
    double BV2 = sqrt(pow((x-a),2) + pow(y,2));     // zelfde nog een keer doen maar nu voor de rechter arm 
    double Dia2 = pow(BV2,2)/(2*BV2); 
    
    //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia2, x, y);
    return Dia2;
}

// calculate Theta1
void CalcTheta1 (double Dia1) {
    double a = 50.0; 
    double Bar = 200.0;    // lengte van de armen 
    
    // Hoek berekenen van het grote tandwiel (gear)
    if (x > -a) {
        Theta1Gear = pi - atan(y/(x+a)) - acos(Dia1/Bar);
    }
    else if (x < -a) {
        Theta1Gear = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
    }
    else {  // als x=-a
        Theta1Gear = 0.5*pi - acos(Dia1/Bar);
    }
    Theta1Gear = Theta1Gear*180.0/pi;       // veranderen van radialen naar graden
             
    // omrekenen van grote tandwiel naar motortandwiel
    Theta1 = Theta1Gear*LargeGear/SmallGear;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12. 
        
    //pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta1, Theta1Gear);
}

void CalcTheta2 (double Dia2) {
    double a = 50.0; 
    double Bar = 200.0;    // lengte van de armen 
    double Prev_Theta2_Gear = Theta2Gear; 
        
    // Hoek berekenen van het grote tandwiel (gear)
    if (x < a) {
        Theta2Gear = pi + atan(y/(x-a)) - acos(Dia2/Bar);
    }
    else if (x > a) {
        Theta2Gear = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
    }
    else {  // als x=a
        Theta2Gear = 0.5*pi - acos(Dia2/Bar);
    }
    Theta2Gear = Theta2Gear*180/pi;         // veranderen van radialen naar graden
    
    // omrekenen van grote tandwiel naar motortandwiel
    Theta2 = Theta2Gear*LargeGear/SmallGear;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.

    //pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
}

// als een van de hoeken te groot wordt, zet dan alles terug naar de vorige positie
void AngleLimits (double Prev_Theta1_Gear, double Prev_Theta2_Gear, double Prev_x, double Prev_y) {
    double MaxThetaGear = 100.0;   // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
    
    if (Theta1Gear >= MaxThetaGear || Theta2Gear >= MaxThetaGear) {
        Theta1Gear = Prev_Theta1_Gear;
        Theta2Gear = Prev_Theta2_Gear;
        x = Prev_x;
        y = Prev_y;
        
        Theta1 = Theta1Gear*LargeGear/SmallGear;
        Theta2 = Theta2Gear*LargeGear/SmallGear; 
    }
    //pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
}

void CalculationsForTheta () {
    sample();
    double Prev_x = Calc_Prev_x ();
    double Prev_y = Calc_Prev_y ();
    double Prev_Theta1_Gear = Calc_Prev_Theta1_Gear ();
    double Prev_Theta2_Gear = Calc_Prev_Theta2_Gear ();
    CalcXY();
    double Dia1 = CalcDia1 (x, y);
    double Dia2 = CalcDia2 (x, y);
    CalcTheta1 (Dia1);
    CalcTheta2 (Dia2);
    //AngleLimits (Prev_Theta1_Gear, Prev_Theta2_Gear, Prev_x, Prev_y);       // laatste check
}    

////////////////////////// Code for control /////////////////////////////////////

double m1_GetPosition()
{
    int countsCW = m1_EncoderCW.getPulses();
    int countsCCW= m1_EncoderCCW.getPulses();
    int net_counts=countsCW-countsCCW;
    double Position=(net_counts*360.0)/COUNTS_PER_REV;
    return Position;
}

double m2_GetPosition()
{
    int countsCW = m2_EncoderCW.getPulses();
    int countsCCW= m2_EncoderCCW.getPulses();
    int net_counts=countsCW-countsCCW;
    double Position=(net_counts*360.0)/COUNTS_PER_REV;
    return Position;
}

// Position control when calibrating
double m1_GetPosition_cal()
{
    int countsCW = m1_EncoderCW.getPulses();
    int countsCCW= m1_EncoderCCW.getPulses();
    int net_counts=countsCW-countsCCW;
    double Position=(net_counts*360.0)/COUNTS_PER_REV+60.0f*LargeGear/SmallGear; // calibrated position is 210 degrees
    return Position;
}

// Position control when calibrating
double m2_GetPosition_cal()
{
    int countsCW = m2_EncoderCW.getPulses();
    int countsCCW= m2_EncoderCCW.getPulses();
    int net_counts=countsCW-countsCCW;
    double Position=(net_counts*360.0)/COUNTS_PER_REV+60.0f*LargeGear/SmallGear; // calibrated position is 210 degrees
    return Position;
}
double m1_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m1_v1, double &m1_v2)
{
    double a1 = -4/(N*Ts+2),
    a2 = -1*(N*Ts - 2)/(N*Ts+2), 
    b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
    b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
    b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
    
    double v = error - a1*m1_v1 - a2*m1_v2;
    double u = b0*v + b1*m1_v1 + b2*m1_v2;
    m1_v2 = m1_v1; m1_v1 = v;
    return u;
}

double m2_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m2_v1, double &m2_v2)
{
    double a1 = -4/(N*Ts+2),
    a2 = -1*(N*Ts - 2)/(N*Ts+2), 
    b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
    b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
    b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
    
    double v = error - a1*m1_v1 - a2*m1_v2;
    double u = b0*v + b1*m1_v1 + b2*m1_v2;
    m1_v2 = m1_v1; m1_v1 = v;
    return u;
}



void SetMotor(int motor_number, double MotorValue)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    if (motor_number == 1)
    {
        if (MotorValue >=0)
        {
            motor1_dir=0;
        }
        else
        {
            motor1_dir=1;
        }
        if (fabs(MotorValue)>1){
            motor1_pwm.write(1);
        }
        else
        {
            motor1_pwm.write(abs(MotorValue));
        }
    }
    else
    {  
        if (MotorValue >=0)
        {
            motor2_dir=0;
        }
        else
        {
            motor2_dir=1;
        }
        if (fabs(MotorValue)>1){
            motor2_pwm.write(1);
        }
        else
        {
            motor2_pwm.write(abs(MotorValue));
        }
    }   
}

void BlinkLed(){
    led = not led;
}

void Controller_motor()
{
    // get the actual position
    double m1_Position = m1_GetPosition_cal();
    double m2_Position = m2_GetPosition_cal();
    // Set position scopes
    scope.set(0,x);
    scope.set(1,y);
    scope.set(2,Theta1);
    scope.set(3,Theta2);
    /*
    scope.set(0,Theta1);
    scope.set(1,m1_Position);
    scope.set(3,Theta2);
    scope.set(4,m2_Position);
    */
    //scope.set(2,m1_Position);
    //scope.set(4,m2_Position);
    // calc the error
    double m1_error = Theta1 - m1_Position;
    double m2_error = Theta2 - m2_Position;
    //scope.set(2,m1_error);
    //scope.set(6,m2_error);
    // calc motorvalues for controller;
    double m1_MotorValue = m1_PID(m1_error, Kp, Ki, Kd, Ts, N, m1_v1, m1_v2);
    double m2_MotorValue = m2_PID(m2_error, Kp, Ki, Kd, Ts, N, m2_v1, m2_v2);
    scope.set(4,m1_MotorValue);
    scope.set(5,m2_MotorValue);
    // Set the motorvalues
    SetMotor(1, m1_MotorValue);
    SetMotor(2, m2_MotorValue);
    // Set motorvalues for scope
    // Send data to HIDScope
    scope.send();
}

void PotControl()
{
    double Motor1_Value = (pot1.read() - 0.5f)*1.5f;
    double Motor2_Value = (pot2.read() - 0.5f)*1.5f;
    //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
    //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
    double m1_Position = m1_GetPosition();
    double m2_Position = m2_GetPosition();
    scope.set(0, Motor1_Value);
    scope.set(1, m1_Position);
    scope.set(2, Motor2_Value);
    scope.set(3, m2_Position);
    scope.send();
    // Write the motors
    SetMotor(1, Motor1_Value);
    SetMotor(2, Motor2_Value);
}

void ResetEncoders()
{
    m1_EncoderCW.reset();
    m1_EncoderCCW.reset();
    m2_EncoderCW.reset();
    m2_EncoderCCW.reset();
}

void Button1Switch()
{
    button1_value = not button1_value;
    pc.printf("\r\n Button1_value = %d \r\n", button1_value);
}

void Button2Switch()
{
    button2_value = not button2_value;
    pc.printf("\r\n Button2_value = %d \r\n", button2_value);
}

void PANIC()
{
    CalculationsTicker.detach();
    PIDControlTicker.detach();
    Stepper_Off();
    SetMotor(1,0.0);
    SetMotor(2,0.0);
    pc.printf("\r\n PANIC BUTTON PRESSED \r\n");
    pc.printf("\r\n TERMINATING ALL PROCESSES \r\n");
}

////////////////////////// main loop ////////////////////////////////////////////
int main()
{
    // Setup
    // Set baud connection with PC
    pc.baud(SERIAL_BAUD);
    pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
    pc.printf("\r\n ***   PROGRAM: CompleteFunction   *** \r\n");
    
    /* making the biquad chain for filtering the emg
    notch and high pass */
    bqc.add( &bq1 ).add( &bq2 );
    bqc2.add( &bq4 ).add( &bq5 );
    bqc3.add( &bq7 ).add( &bq8 );
        
    // Setup Blinking LED to blink every 0.5 sec
    led = 1;
    BlinkLedTicker.attach(BlinkLed,0.5);
    
    // Setup other leds
    red = 1;
    blue = 1;
    green = 1;
    
    // Setup motor PWM speeds
    motor1_pwm.period(1.0/1000);
    motor2_pwm.period(1.0/1000);
    
    // Setup Interruptin.fall
    button1.fall(Button1Switch);
    button2.fall(PANIC);
    button3.fall(Stepper_On);
    //button4.fall(xxx);
        
    // Actual code starts here
    // Start positioning the arms
    pc.printf("\r\n ***************** \r");
    pc.printf("\r\n Press Button 1 to start positioning the arms using PotControl\r");
    pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r");
    pc.printf("\r\n ***************** \r\n");
    //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
    while (button1_value == false)
    {   
        wait(2);
        //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
        //wait(5);
    }
    PotControlTicker.attach(&PotControl,CONTROLLER_TS);
    
    pc.printf("\r\n ***************** \r");
    pc.printf("\r\n When done positioning, press Button 1 to detach Potcontrol");
    pc.printf("\r\n ***************** \r\n");
    while (button1_value == true)
    {   
        //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
        wait(2);
    }
    
    // Detach potcontrol
    PotControlTicker.detach();
    
    // Set motors to 0
    SetMotor(1,0.0);
    SetMotor(2,0.0);
    // Wait a bit to let the movement stop
    wait(0.5);
    // Reset the encoders to set the 0 position
    ResetEncoders();
    
    // PID control starts
    pc.printf("\r\n ***************** \r");
    pc.printf("\r\n When done positioning, press Button 1 to start potmeter PID control");
    pc.printf("\r\n Make sure both potentiometers are positioned halfway!!!");
    pc.printf("\r\n ***************** \r\n");
    while (button1_value == false)
    {   
        //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
        wait(2);
    }
    
    // Attach sample, calc and control tickers
    CalculationsTicker.attach(&CalculationsForTheta,CALC_TS);
    PIDControlTicker.attach(&Controller_motor, CONTROLLER_TS);  
    while(true){}
}