File content as of revision 29:78419e287e62:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include <iostream>

//Inputs and outputs
MODSERIAL pc(USBTX, USBRX);
QEI motor2_pos (D8, D9, NC, 32);
QEI motor1_pos (D12, D13, NC, 32);
AnalogIn ain2(A2);
AnalogIn ain1(A3);
DigitalOut dir2(D4);
DigitalOut dir1(D7);
//D4,D7 direction of motors 2,1 on board, D5,D6- PWM of motors 2,1 on board
PwmOut motor2_pwm(D5);
PwmOut motor1_pwm(D6);
AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );

Ticker ticktick;
Timer state_time;
Timeout EMG_peakx;
Timeout turnx;
Timeout EMG_peaky;
Timeout turny;
Ticker      sample_timer;
HIDScope    scope( 6);
DigitalOut  ledred(LED_RED);
DigitalOut  ledblue(LED_BLUE);
DigitalOut  ledgreen(LED_GREEN);
InterruptIn err(SW2);
InterruptIn button(SW3);
InterruptIn left_button(D3);
InterruptIn right_button(D2);

//variables
volatile float P0;
volatile float P1;
volatile float EMG_min0=1;
volatile float EMG_max0=0;
volatile float EMG_min1=1;
volatile float EMG_max1=0;
volatile bool ignore_peaksx=false;
volatile bool ignore_turnx=true;
volatile bool ignore_peaksy=false;
volatile bool ignore_turny=true;
enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
states currentState=Waiting;
volatile bool motor1_calibrated=false;

volatile bool motorx_on=false;
volatile float signx=0.075;
volatile bool motory_on=false;
volatile float signy=0.075;

volatile bool demo_done=false;
volatile float x_ref=0.175;
volatile float y_ref=0.175;
static float v_refx; //reference speeds 
static float v_refy;
static float T = 0.002; //measuring period
static float L = 0.35;  //distance between the motor axles, has to be checked
volatile float theta1_old = 0.0; //feedback variables
volatile float theta2_old = 0.0;


float error_1;
float error_2;
volatile float filtered_error_derivativex;
volatile float filtered_error_derivativey;

//PID controller variables
bool q = true;
float error_prev;
volatile float Kp = 24.6;
volatile float Ki = 28.3;
volatile float Kd = 0.1;
float u;
float u_p;
float u_i;
float u_d;
float error_integral1 = 0.0;
float error_integral2 = 0.0;

float u_1;
float u_2;
const float angle2_offset=asin(0.2);
const float angle1_offset=atan(2.2/32.8);
const double pi=3.1415926535897932384626;
volatile float theta1;
volatile float theta2;
volatile float theta1_ref;
volatile float theta2_ref;

void read_emg()//read and filter EMG
{
    //EMG signal 0
    static int count0=0;
    static float RMS_value0=0;
    static float HighPass_value0=0;
    count0+=1;
    static float RMS0[150];
    static float HighPass0[30];
    static BiQuad Notch0(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
    static BiQuad Notch1(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
    float I0;
    float If0;
    //signal 1
    static int count1=0;
    static float RMS_value1=0;
    static float HighPass_value1=0;
    count1+=1;
    static float RMS1[150];
    static float HighPass1[30];
    float I1;
    float If1;
    I0=emg0.read(); //read signal
    double notched0=Notch0.step(I0);
    HighPass_value0+=(notched0-HighPass0[count0%30])/30.0;
    HighPass0[count0%30]=notched0;
    If0=pow(I0-HighPass_value0,2.0f); // Highpass-filtered value squared
    RMS_value0+=(If0-RMS0[count0%150])/150.0;
    RMS0[count0%150]=If0;
    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    P0=sqrt(RMS_value0);
    I1=emg1.read(); //read signal
    double notched1=Notch1.step(I1);
    HighPass_value1+=(notched1-HighPass1[count1%30])/30.0;
    HighPass1[count1%30]=notched1;
    If1=pow(I1-HighPass_value1,2.0f); // Highpass-filtered value squared
    RMS_value1+=(If1-RMS1[count1%150])/150.0;
    RMS1[count1%150]=If1;
    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    P1=sqrt(RMS_value1);
    /* 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 */
    /* To indicate that the function is working, the LED is toggled */
    ledred=1;
    ledgreen=0;
    ledblue=1;
}

void get_angles(void) //calculate angles from encoder output
{
    float pulses1=motor1_pos.getPulses();
    float pulses2=motor2_pos.getPulses();
    theta1=angle1_offset+pulses1*17.0/16.0*2*pi/131.0/32.0;
    theta2=angle2_offset+pulses2*17.0/16.0*2*pi/131.0/32.0; 
}

void pos_cal(void) // main function for calibrating the motors
{
    float t=state_time.read();
    static int pos_time_counter=0;
    static int last_ticks=10000;
    float pulses;
    pos_time_counter+=1;
    if(!motor1_calibrated&&t>1.0f)
    {
        dir1=1; //???
        motor1_pwm.write(0.7f);
        pulses=motor1_pos.getPulses();
        if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
        {
            ledblue=!ledblue;
            motor1_pos.reset();
            last_ticks=10000;
        }
        else if(pos_time_counter%500==0)
        {
            last_ticks=motor1_pos.getPulses();
        }
        
    }
    else if(!motor1_calibrated)
    {
        motor2_pwm.write(0.7f);
        dir2=1;
    }
    else if(t>1.0f)
    {
        motor1_pwm.write(0.0f);
        dir2=1; //???
        motor2_pwm.write(0.7f);
        pulses=motor2_pos.getPulses();
        if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
        {
            ledblue=!ledblue;
            motor2_pos.reset();
            motor2_pwm.write(0.0f);
        }
        else if(pos_time_counter%500==0)
        {
            last_ticks=motor2_pos.getPulses();
        }
    }   
    
}

void record_min_max(void)// main function for calibrating EMG
{
    float t=state_time.read();
    if(t>0.4)
    {
        if(P0<EMG_min0)
        {
            EMG_min0=P0;
        }
        else if(P0>EMG_max0)
        {
            EMG_max0=P0;
        }
        if(P1<EMG_min1)
        {
            EMG_min1=P1;
        }
        else if(P1>EMG_max1)
        {
            EMG_max1=P1;
        }
    }
}

//timed functions for EMG control
void unignore_peaksx(void)
{
    ignore_peaksx=false;
}
void start_ignore_turnx(void)
{
    ignore_turnx=true;
}
void unignore_peaksy(void)
{
    ignore_peaksy=false;
}
void start_ignore_turny(void)
{
    ignore_turny=true;
}

void set_v_refxy(void)//set reference velocities based on EMG input
{
   float Q0;
   Q0=2.0f*(P0-(EMG_min0+EMG_max0)/2.0f)/(EMG_max0-EMG_min0);
   float Q1;
   Q1=2.0f*(P1-(EMG_min1+EMG_max1)/2.0f)/(EMG_max1-EMG_min1);
   if (Q0>-0.2f && !ignore_peaksx)
   {
        if (motorx_on)
        {
            v_refx=0;
            EMG_peakx.attach(unignore_peaksx,0.8);
            turnx.attach(start_ignore_turnx,1);
            ignore_turnx=false;
            ignore_peaksx=true;
            motorx_on=false;
        }
        else if(ignore_turnx)
        {
            v_refx=1.0*signx;
            EMG_peakx.attach(unignore_peaksx,0.8);
            turnx.attach(start_ignore_turnx,1);
            ignore_turnx=false;
            ignore_peaksx=true;
            motorx_on=true;
        }
        else
        {
            signx=-1.0*signx;
            v_refx=1.0*signx;
            EMG_peakx.attach(unignore_peaksx,0.8);
            ignore_peaksx=true;
            motorx_on=true;
        } 
    }
   if (Q1>-0.2f && !ignore_peaksy)
   {
        if (motory_on)
        {
            v_refy=0;
            EMG_peaky.attach(unignore_peaksy,0.8);
            turny.attach(start_ignore_turny,1);
            ignore_turny=false;
            ignore_peaksy=true;
            motory_on=false;
        }
        else if(ignore_turny)
        {
            v_refy=1.0*signy;
            EMG_peaky.attach(unignore_peaksy,0.8);
            turny.attach(start_ignore_turny,1);
            ignore_turny=false;
            ignore_peaksy=true;
            motory_on=true;
        }
        else
        {
            signy=-1.0*signy;
            v_refy=1.0*signy;
            EMG_peaky.attach(unignore_peaksy,0.8);
            ignore_peaksy=true;
            motory_on=true;
        } 
    }
    float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
    float y_pos=tan(theta1)*x_pos;
    if(x_pos>=0.35&&v_refx>0)
    {
        v_refx=0;
    }
    else if(x_pos<=0.0&&v_refx<0)
    {
        v_refx=0;
    }
    if(y_pos>=0.45&&v_refy>0)
    {
        v_refy=0;
    }
    else if(y_pos<=0.07&&v_refy<0)
    {
        v_refy=0;
    }
}

void left_fake_emg(void)// optional- set refence x direction based on pressing a button
{
    if (!ignore_peaksx)
   {
        if (motorx_on)
        {
            v_refx=0;
            EMG_peakx.attach(unignore_peaksx,0.8);
            turnx.attach(start_ignore_turnx,1);
            ignore_turnx=false;
            ignore_peaksx=true;
            motorx_on=false;
        }
        else if(ignore_turnx)
        {
            v_refx=1.0*signx;
            EMG_peakx.attach(unignore_peaksx,0.8);
            turnx.attach(start_ignore_turnx,1);
            ignore_turnx=false;
            ignore_peaksx=true;
            motorx_on=true;
        }
        else
        {
            signx=-1.0*signx;
            v_refx=1.0*signx;
            EMG_peakx.attach(unignore_peaksx,0.8);
            ignore_peaksx=true;
            motorx_on=true;
        } 
    }
}

void right_fake_emg(void)// optional- set refence y direction based on pressing a button
{
    if (!ignore_peaksy)
   {
        if (motory_on)
        {
            v_refy=0;
            EMG_peaky.attach(unignore_peaksy,0.8);
            turny.attach(start_ignore_turny,1);
            ignore_turny=false;
            ignore_peaksy=true;
            motory_on=false;
        }
        else if(ignore_turny)
        {
            v_refy=1.0*signy;
            EMG_peaky.attach(unignore_peaksy,0.8);
            turny.attach(start_ignore_turny,1);
            ignore_turny=false;
            ignore_peaksy=true;
            motory_on=true;
        }
        else
        {
            signy=-1.0*signy;
            v_refy=1.0*signy;
            EMG_peaky.attach(unignore_peaksy,0.8);
            ignore_peaksy=true;
            motory_on=true;
        } 
    }
}

void set_ref_fake_emg(void) // optional- set refence x/y velocity based on pressing a button
{
    float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
    float y_pos=tan(theta1)*x_pos;
    if(x_pos>=0.35&&v_refx>0)
    {
        v_refx=0;
    }
    else if(x_pos<=0.0&&v_refx<0)
    {
        v_refx=0;
    }
    if(y_pos>=0.45&&v_refy>0)
    {
        v_refy=0;
    }
    else if(y_pos<=0.07&&v_refy<0)
    {
        v_refy=0;
    }
}

void error(void){ //calculate the errors on the motor angles based on input reference velocities or positions
    float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
    float y_pos=tan(theta1)*x_pos;
    float r1=sqrt(pow(x_pos,2)+pow(y_pos,2));
    float r2=sqrt(pow(0.35-x_pos,2)+pow(y_pos,2));
    float J11=-r1*cos(theta2)/sin(theta1+theta2);
    float J21=r1*sin(theta2)/sin(theta1+theta2);
    float J12=r2*cos(theta1)/sin(theta1+theta2);
    float J22=r2*sin(theta1)/sin(theta1+theta2);
    float a=J22/(J11*J22-J12*J21);
    float b=-J12/(J11*J22-J12*J21);
    float c=-J21/(J11*J22-J12*J21);
    float d=J11/(J11*J22-J12*J21);
    /*
    float dvd=L * (tan(theta2) * pow(tan(theta1),2) + tan(theta1) * pow(tan(theta2),2));//+ is aangepast

    float a =  (  -pow(cos(theta1),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta1),2)  )  / dvd;             // inverse matrix components of differential x and differential y
    float b =  (   pow(cos(theta1),2) * pow((tan(theta1)+tan(theta2)),2) * tan(theta1)         )  / dvd; 
    float c =  (   pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2)  )  / dvd;
    float d =  (   pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2)  )  / dvd;
    */
    float theta1_dot = a*v_refx + b*v_refy;
    float theta2_dot = c*v_refx + d*v_refy;
    
    if(currentState==Operating||currentState==Demo)
    {
        theta1_ref = theta1_old+theta1_dot*T;
        theta2_ref = theta2_old+theta2_dot*T;
        if(theta1_ref<theta1-0.2)
        {
            theta1_ref=theta1-0.2;
        }
        else if(theta1_ref>theta1+0.2)
        {
            theta1_ref=theta1+0.2;
        }
        if(theta2_ref<theta2-0.2)
        {
            theta2_ref=theta2-0.2;
        }
        else if(theta2_ref>theta2+0.2)
        {
            theta2_ref=theta2+0.2;
        }
        if(theta1_ref<0.1f)
        {
            theta1_ref=0.1f;
        }
        else if(theta1_ref>1.8f)
        {
            theta1_ref=1.8f;
        }
        if(theta2_ref<0.1f)
        {
            theta2_ref=0.1f;
        }
        else if(theta2_ref>1.8f)
        {
            theta2_ref=1.8f;
        }
    }
    else if(currentState==Homing)
    {
        theta1_ref=pi/4.0f;
        theta2_ref=pi/4.0f;
    }
    /*
    else if(currentState==Demo)
    {
        theta1_ref=atan(y_ref/x_ref);
        theta2_ref=atan(y_ref/(0.35f-x_ref));
    }
    */
    error_1 = theta1 - theta1_ref;
    error_2 = theta2 - theta2_ref;
    
    theta1_old = theta1_ref;
    theta2_old = theta2_ref;
    
    }
    
float PID1(float err){//Calculate controller output for motor 1 based on error 1
    //P action
    u_p = Kp * err;
    float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
    float y_pos=tan(theta1)*x_pos;
    float In1=0.25f*0.49/3.0f+0.2f*(pow(x_pos,2)+pow(y_pos,2));
    float r1=0.1+sqrt(pow(x_pos,2)+pow(y_pos,2));
    //I action 
    error_integral1 = error_integral1 + err * T;
    u_i = Ki * error_integral1;
    
    //D action
    static float error_prevx=0;
    static float error_derivativex;
    static BiQuad LowPassFilterx(0.1311f,0.2622f,0.1311f,-0.7478,0.2722f);
    
        
    error_derivativex = (err - error_prevx)/T;
    filtered_error_derivativex = LowPassFilterx.step(error_derivativex);
    u_d = Kd * filtered_error_derivativex;
    error_prevx = err;
    
    u = r1/3.0f*(u_p + u_i+u_d);
    if(filtered_error_derivativex>0.3)
    {
        u=0;
    }
    return u;
    }
    
float PID2(float err){//Calculate controller output for motor 2 based on error 2
    //P action
    u_p = Kp * err;
    float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
    float y_pos=tan(theta1)*x_pos;
    float In2=0.25f*0.49/3.0f+0.2f*(pow(0.35-x_pos,2)+pow(y_pos,2));
    float r2=0.1+sqrt(pow(0.35f-x_pos,2)+pow(y_pos,2));
    //I action 
    error_integral2 = error_integral2 + err * T;
    u_i = Ki * error_integral2;
    

    //D action
    static float error_prevy=0;
    static float error_derivativey;
    static BiQuad LowPassFiltery(0.1311f,0.2622f,0.1311f,-0.7478,0.2722f);
    
        
    error_derivativey = (err - error_prevy)/T;
    filtered_error_derivativey = LowPassFiltery.step(error_derivativey);
    u_d = Kd * filtered_error_derivativey;
    error_prevy = err;
    
    
    u = r2/3.0f*(u_p + u_i+u_d);
    return u;
    }
void set_refxy(void)//set reference velocities in the demo mode
{
    float t=state_time.read();
    int tfloor=floor(t)/1;
    float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
    float y_pos=tan(theta1)*x_pos;
    static int demo_subpart=1;
    float v_demo=0.08;
    /* Rectangle
    if(tfloor%12==0||tfloor%12==1||tfloor%12==2)
    {
        x_ref=0.025+0.1*(t-tfloor+tfloor%12);
        y_ref=0.2;
    }
    else if(tfloor%12==3||tfloor%12==4||tfloor%12==5)
    {
        x_ref=0.325;
        y_ref=0.2+0.05*(t-tfloor+tfloor%12-3);
    }
    else if(tfloor%12==6||tfloor%12==7||tfloor%12==8)
    {
        x_ref=0.325-0.1*(t-tfloor+tfloor%12-6);
        y_ref=0.35;
    }
    else
    {
        x_ref=0.025;
        y_ref=0.35-0.05*(t-tfloor+tfloor%12-9);
    }
    */
    /* Circle
    x_ref=0.175f+0.15*cos(t/2.5f);
    y_ref=0.275f+0.15*sin(t/2.5f);
    */
    if(tfloor<5)
    {
        theta1_old=pi/4;
        theta2_old=pi/4;
    }
    else if(tfloor<12)
    {
        x_ref=0.025+(x_pos-0.025)*(12-t)/7.0f;
        y_ref=0.1+(y_pos-0.1)*(12-t)/7.0f;
        theta1_old=atan(y_ref/x_ref);
        theta2_old=atan(y_ref/(0.35f-x_ref));
    }
    else if(demo_subpart==1&&y_pos<0.4)
    {
        v_refx=0;
        v_refy=v_demo;
    }
    else if(demo_subpart==2&&x_pos<0.075)
    {
        v_refx=v_demo;
        v_refy=0;
    }
    else if(demo_subpart==3&&y_pos>0.05)
    {
        v_refx=0;
        v_refy=-v_demo;
    }
    else if(demo_subpart==4&&x_pos<0.125)
    {
        v_refx=v_demo;
        v_refy=0;
    }
    else if(demo_subpart==5&&y_pos<0.05)
    {
        v_refx=0;
        v_refy=v_demo;
    }
    else if(demo_subpart==6&&x_pos<0.175)
    {
        v_refx=v_demo;
        v_refy=0;
    }
    else if(demo_subpart==7&&y_pos>0.05)
    {
        v_refx=0;
        v_refy=-v_demo;
    }
    else if(demo_subpart==8&&x_pos<0.225)
    {
        v_refx=v_demo;
        v_refy=0;
    }
    else if(demo_subpart==9&&y_pos<0.4)
    {
        v_refx=0;
        v_refy=v_demo;
    }
    else if(demo_subpart==10&&x_pos<0.275)
    {
        v_refx=v_demo;
        v_refy=0;
    }
    else if(demo_subpart==11&&y_pos>0.05)
    {
        v_refx=0;
        v_refy=-v_demo;
    }
    else if(demo_subpart==12&&x_pos<0.325)
    {
        v_refx=v_demo;
        v_refy=0;
    }
    else if(demo_subpart==13&&y_pos<0.4)
    {
        v_refx=0;
        v_refy=v_demo;
    }
    else if(demo_subpart==14)
    {
        demo_done=true;
        wait(10);   
        demo_done=false;
        demo_subpart=1;
        state_time.reset();
    }
    else
    {
        demo_subpart+=1;
    }
    //float asdfgh=x_ref;
    //x_ref=y_ref-0.075;
    //y_ref=asdfgh+0.125;
}
 
void setPWM_controller(void){//set motor PWM values based on controller output
    error();
    u_1 = PID1(error_1);
    u_2 = PID2(error_2);
    
    if(u_1 < 0.0f){
        dir1 = 0;
    }else{
        dir1 = 1;
        }
    motor1_pwm.write(min(0.5f+0.5f*fabs(u_1),1.0f));
    
    if(u_2 < 0.0f){
        dir2 = 0;
    }else{
        dir2 = 1;
        }
    motor2_pwm.write(min(0.5f+0.5f*fabs(u_2),1.0f));
    
    }

void sample()// main funtion loop
{
    get_angles();
    scope.set(0,theta1);
    scope.set(1,theta2);
    scope.set(2,v_refx);
    scope.set(3,v_refy);
    scope.set(4,theta1_ref);
    scope.set(5,theta2_ref);
    scope.send(); 
    switch(currentState)
    {
        case Waiting:
            ledred=0;
            ledgreen=0;
            ledblue=1;
            break;
        case Position_calibration:
            ledred=1;
            ledgreen=1;
            ledblue=0;
            pos_cal();
            break;
        case EMG_calibration:
            ledred=1;
            ledgreen=0;
            ledblue=0;
            read_emg();
            record_min_max();
            break;
        case Homing:
            setPWM_controller();
            ledred=0;
            ledgreen=1;
            ledblue=0;
            break;
        case Operating:
            read_emg();
            set_v_refxy();
            set_ref_fake_emg();
            setPWM_controller();
            ledred=1;
            ledgreen=0;
            ledblue=1;
            break;
        case Demo:
            if(!demo_done)
            {
                set_refxy();
                setPWM_controller();
            }
            else
            {
                motor1_pwm.write(0.0f);
                motor2_pwm.write(0.0f);
            }
            ledred=0;
            ledgreen=0;
            ledblue=0;
            break;
        case Failure:
            ledred=0;
            ledgreen=1;
            ledblue=1;
            motor1_pwm.write(0.0);
            motor2_pwm.write(0.0);
            break;
    }
    //Preventing the machine from breaking
}

void error_occur()//executed when error button is pressed
{
    currentState=Failure;
}

void return_home()//main function for homing state
{
    theta1_ref=pi/4.0f;
    theta2_ref=pi/4.0f;
    theta1_old=pi/4.0f;
    theta2_old=pi/4.0f;
    v_refx=0;
    EMG_peakx.attach(unignore_peaksx,0.8);
    ignore_peaksx=true;
    motorx_on=false;
    v_refy=0;
    EMG_peaky.attach(unignore_peaksy,0.8);
    ignore_peaksy=true;
    motory_on=false;
}

void button_press(void) 
//Press button to change state
{
    state_time.reset();
    switch(currentState)
    {
        case Waiting:
            currentState=Position_calibration;
            wait(1);
            break;
        case Position_calibration:
            if(!motor1_calibrated)
            {
                motor1_calibrated=true;
                state_time.reset();
                dir1=!dir1;
                motor1_pwm.write(0.0f);
            }
            else
            {
                currentState=EMG_calibration;
                motor2_pwm.write(0.0f);
                motor1_pwm.write(0.0f);
            }   
            wait(1);
            break;
        case EMG_calibration:
            currentState=Homing;
            wait(1);
            break;
        case Homing:
            currentState=Operating;
            wait(1);
            break;
        case Operating:
            currentState=Demo;
            wait(1);
            break;
            
        case Demo:
            currentState=Operating;
            wait(1);
            break;
    }
}

int main()
{
    pc.baud(115200);
    pc.printf("Starting...");
    ledred=0;
    sample_timer.attach(&sample, 0.002);
    err.fall(error_occur);
    button.fall(button_press);
    left_button.fall(left_fake_emg);
    left_button.rise(left_fake_emg);
    right_button.fall(return_home);
    right_button.rise(return_home);
    int frequency_pwm=21000;
    motor1_pwm.period(1.0/frequency_pwm);
    motor2_pwm.period(1.0/frequency_pwm);
    state_time.start();
    while (true) {
        wait(10);
    }

}