File content as of revision 0:58c11abe4785:

// Hello World! for the TextLCD*//

#include "mbed.h"
#include "TextLCD.h"
#include "MODSERIAL.h"
#include "encoder.h"

// definieren constanten
#define PI 3.1415926
//plant
#define ARM1 0.36
#define ARM2 0.26
//PD
#define CI 0.012
#define CP 0.001
#define CD 0.02
#define CLP1 0.9975
#define CLP2 0.001
//Snelheid
#define SNELHEID 0.05
//LOOPTIME
#define LOOPTIME 0.006667
//Filtering EMG
#define HP1 0.905
#define HP2 20.0
#define HP3 20.0
#define LP1 0.9753
#define LP2 0.0247
//EMG threshold
#define SET_EMG_MAX1        3.0 //bovenarm rechts > beweging naar rechts
#define SET_EMG_MIN1        1.5
#define SET_EMG_MAX2        2.0 //bovenarm links  > beweging naar links
#define SET_EMG_MIN2        0.75
#define SET_EMG_MAX3        3.5 //onderarm rechts  > beweging naar boven
#define SET_EMG_MIN3        1.5
#define SET_EMG_MAX4        2.0 //onderarm links  > beweging naar onder
#define SET_EMG_MIN4        1.0

void aansturing(void);
void uitzetten(void);
void setlooptimerflag(void);
void keep_in_range(float * in, float min, float max);

volatile bool looptimerflag;

Serial pc(USBTX, USBRX);
TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9, TextLCD::LCD16x2,NC,NC,TextLCD::HD44780); // rs, e, d4-d7-/*+-9
AnalogIn EMG1(PTB0);                    //EMG
AnalogIn EMG2(PTB1);
AnalogIn EMG3(PTB2);
AnalogIn EMG4(PTB3);
AnalogIn potmeter(PTC2);                //potmeter
DigitalIn ButtonSTOP(PTE21);            //Knopjes voor kalibratie
DigitalIn ButtonSELECT(PTE20);
DigitalIn ButtonUP(PTE23);
DigitalIn ButtonDOWN(PTE22);
DigitalOut Solenoid(PTD4);              //Solenoid
Encoder motor1(PTD0,PTC8);              //Encoder
Encoder motor2(PTD2,PTC9);
PwmOut pwm_motor1(PTA12);               //motor
DigitalOut motordir1(PTD3);
PwmOut pwm_motor2(PTA5);
DigitalOut motordir2(PTD1);

float numberx = 9;
int menu=0, t;
float EMGmax1=SET_EMG_MAX1, EMGmin1=SET_EMG_MIN1, EMGmax2=SET_EMG_MAX2, EMGmin2=SET_EMG_MIN2;
float EMGmax3=SET_EMG_MAX3, EMGmin3=SET_EMG_MIN3, EMGmax4=SET_EMG_MAX4, EMGmin4=SET_EMG_MIN4;
float drawspeed=SNELHEID;


//Variabelen verwerking EMG
    float emg_value1, emg_value2, emg_value3, emg_value4;
    float emg_value1min1=0.5, emg_value2min1=0.5, emg_value3min1=0.5, emg_value4min1=0.5;
    float EMGhp1, EMGhp2, EMGhp3, EMGhp4, EMGlp1, EMGlp2, EMGlp3, EMGlp4;
    float EMGhp1min1=0.5, EMGhp2min1=0.5, EMGhp3min1=0.5, EMGhp4min1=0.5, EMGlp1min1=0.5, EMGlp2min1=0.5, EMGlp3min1=0.5, EMGlp4min1=0.5;

    //Variabelen bepaling input systeem
    float input;
    float w1, w2, wM2, phi1, phi2, theta;
    float a, b, c, d, ai, bi, ci, di;
    float v1, v2, v3, v4, vx, vy, snelheid;
    float M1position, M2position, M2phi;
    float Px, Py;
    
    //Variabelen motoraansturing
    float setpointM1, setpointM2;
    float setpointmin1M1=800.0, setpointmin1M2=2400.0;
    float pwm_to_motor1, pwm_to_motor2;
    float foutM1, foutM2;
    float foutmin1M1=0.0, foutmin1M2=0.0;
    float foutverschilM1, foutverschilM2;
    float foutverschilmin1M1=0.0, foutverschilmin1M2=0.0;
    float foutImin1=0.0, foutImin2=0.0, foutI1, foutI2;
    float CDloop=CD/LOOPTIME;
    float t_sin=0.0;
    float t_timer=0.0;

int main() {
    //set buttons PULLDOWN
    ButtonSTOP.mode(PullNone);
    ButtonSELECT.mode(PullNone);
    ButtonUP.mode(PullNone);
    ButtonDOWN.mode(PullNone);
    pc.baud(57600);
    //Aanstuur timing
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,LOOPTIME); 
    while(1)
    {
        switch (menu) 
        {
        case 0:
            lcd.cls();
            lcd.printf("> DRAW");
            lcd.locate(0,1);
            lcd.printf("  SETTINGS");
            while(menu==0)
            {
                if (ButtonDOWN.read()==1)   menu++;
                if (ButtonSELECT.read()==1) 
                {
                    motor1.setPosition(800);
                    motor2.setPosition(2400);
                    menu=55;
                    lcd.cls();
                    lcd.printf("  Drawing ...");
                    lcd.locate(0,1);
                    lcd.printf("UP: Pause");
                }
            }
        break;    
        case 1:
            lcd.cls();
            lcd.printf("> SETTINGS");
            lcd.locate(0,1);
            lcd.printf("  RESET ALL");
            while(menu==1)
            {
                if (ButtonUP.read()==1)     menu--;
                if (ButtonDOWN.read()==1)   menu++;
                if (ButtonSELECT.read()==1) menu=20;
            }
        break;
        case 2:
            lcd.cls();
            lcd.printf("> RESET ALL");
            lcd.locate(0,1);
            lcd.printf("           ");
            while(menu==2)
                {
                if (ButtonUP.read()==1)     menu--;
                if (ButtonSELECT.read()==1)
                {
                    EMGmax1=SET_EMG_MAX1;   EMGmin1=SET_EMG_MIN1;
                    EMGmax2=SET_EMG_MAX2;   EMGmin2=SET_EMG_MIN2;
                    EMGmax3=SET_EMG_MAX3;   EMGmin3=SET_EMG_MIN3;
                    EMGmax4=SET_EMG_MAX4;   EMGmin4=SET_EMG_MIN4;
                    drawspeed=SNELHEID;
                    lcd.locate(0,1);
                    lcd.printf(" Reset Completed");
                    wait(1);
                    lcd.locate(0,1);
                    lcd.printf("                ");
                }
            }
        break;
        case 20:
            lcd.cls();
            lcd.printf("> EMG1-MAX: %.2f", (EMGmax1+ (((potmeter.read()+0.0005)*2)-1)));
            lcd.locate(0,1);
            lcd.printf("  EMG1-MIN: ");
            if (ButtonSTOP.read()==1) menu=0;
            if (ButtonDOWN.read()==1) menu++;
            if (ButtonSELECT.read()==1)
                {
                lcd.locate(0,1);
                lcd.printf("  SAVED!      ");
                EMGmax1=(EMGmax1+(((potmeter.read()+0.0005)*2)-1));
                wait(0.5);
                }
        break;
        case 21:
            lcd.cls();
            lcd.printf("> EMG1-MIN: %.2f", (EMGmin1+ (((potmeter.read()+0.0005)*2)-1)));
            lcd.locate(0,1);
            lcd.printf("  EMG2-MAX: ");
            if (ButtonSTOP.read()==1)   menu=0;
            if (ButtonUP.read()==1)     menu--;
            if (ButtonDOWN.read()==1)   menu++;
            if (ButtonSELECT.read()==1)
                {
                lcd.locate(0,1);
                lcd.printf("  SAVED!      ");
                EMGmin1=(EMGmin1+ (((potmeter.read()+0.0005)*2)-1));
                wait(0.5);
                }
        break;
        case 22:
            lcd.cls();
            lcd.printf("> EMG2-MAX: %.2f", (EMGmax1+ (((potmeter.read()+0.0005)*2)-1)));
            lcd.locate(0,1);
            lcd.printf("  EMG2-MIN: ");
            if (ButtonSTOP.read()==1)   menu=0;
            if (ButtonUP.read()==1)     menu--;
            if (ButtonDOWN.read()==1)   menu++;
            if (ButtonSELECT.read()==1)
                {
                lcd.locate(0,1);
                lcd.printf("  SAVED!      ");
                EMGmax2=(EMGmax1+ (((potmeter.read()+0.0005)*2)-1));
                wait(0.5);
                }
        break;
        case 23:
            lcd.cls();
            lcd.printf("> EMG2-MIN: %.2f", (EMGmin2+ (((potmeter.read()+0.0005)*2)-1)));
            lcd.locate(0,1);
            lcd.printf("  EMG3-MAX: ");
            if (ButtonSTOP.read()==1)   menu=0;
            if (ButtonUP.read()==1)     menu--;
            if (ButtonDOWN.read()==1)   menu++;
            if (ButtonSELECT.read()==1)
                {
                lcd.locate(0,1);
                lcd.printf("  SAVED!      ");
                EMGmin2=(EMGmin2+ (((potmeter.read()+0.0005)*2)-1));
                wait(0.5);
                }
        break;
        case 24:
            lcd.cls();
            lcd.printf("> EMG3-MAX: %.2f", (EMGmax3+ (((potmeter.read()+0.0005)*2)-1)));
            lcd.locate(0,1);
            lcd.printf("  EMG3-MIN: ");
            if (ButtonSTOP.read()==1)   menu=0;
            if (ButtonUP.read()==1)     menu--;
            if (ButtonDOWN.read()==1)   menu++;
            if (ButtonSELECT.read()==1)
                {
                lcd.locate(0,1);
                lcd.printf("  SAVED!      ");
                EMGmax3=(EMGmax3+ (((potmeter.read()+0.0005)*2)-1));
                wait(0.5);
                }
        break;
        case 25:
            lcd.cls();
            lcd.printf("> EMG3-MIN: %.2f", (EMGmin3+ (((potmeter.read()+0.0005)*2)-1)));
            lcd.locate(0,1);
            lcd.printf("  EMG4-MAX: ");
            if (ButtonSTOP.read()==1)   menu=0;
            if (ButtonUP.read()==1)     menu--;
            if (ButtonDOWN.read()==1)   menu++;
            if (ButtonSELECT.read()==1)
                {
                lcd.locate(0,1);
                lcd.printf("  SAVED!      ");
                EMGmin3=(EMGmin3+ (((potmeter.read()+0.0005)*2)-1));
                wait(0.5);
                }
        break;
        case 26:
            lcd.cls();
            lcd.printf("> EMG4-MAX: %.2f", (EMGmax4+ (((potmeter.read()+0.0005)*2)-1)));
            lcd.locate(0,1);
            lcd.printf("  EMG4-MIN: ");
            if (ButtonSTOP.read()==1)   menu=0;
            if (ButtonUP.read()==1)     menu--;
            if (ButtonDOWN.read()==1)   menu++;
            if (ButtonSELECT.read()==1)
                {
                lcd.locate(0,1);
                lcd.printf("  SAVED!      ");
                EMGmax4=(EMGmax4+ (((potmeter.read()+0.0005)*2)-1));
                wait(0.5);
                }
        break;
        case 27:
            lcd.cls();
            lcd.printf("> EMG4-MIN: %.2f", (EMGmin4+ (((potmeter.read()+0.0005)*2)-1)));
            lcd.locate(0,1);
            lcd.printf("  SPEED   :");
            if (ButtonSTOP.read()==1)   menu=0;
            if (ButtonUP.read()==1)     menu--;
            if (ButtonDOWN.read()==1)   menu++;
            if (ButtonSELECT.read()==1)
                {
                lcd.locate(0,1);
                lcd.printf("  SAVED!      ");
                EMGmin4=(EMGmin4+ (((potmeter.read()+0.0005)*2)-1));
                wait(0.5);
                }
        break;
        case 28:
            lcd.cls();
            lcd.printf("> SPEED  : %.2f", (drawspeed+ (((potmeter.read()+0.0005)/10)-0.05)));
            lcd.locate(0,1);
            lcd.printf("  SOLENOID:");
            if (ButtonSTOP.read()==1)   menu=0;
            if (ButtonUP.read()==1)     menu--;
            if (ButtonDOWN.read()==1)   menu++;
            if (ButtonSELECT.read()==1)
                {
                lcd.locate(0,1);
                lcd.printf("  SAVED!      ");
                drawspeed=(drawspeed+ (((potmeter.read()+0.0005)/10)-0.05));
                wait(0.5);
                }
        break;
        case 29:
            lcd.cls();
            lcd.printf("> SOLENOID: OFF");
            lcd.locate(0,1);
            lcd.printf("           ");
            if (ButtonSTOP.read()==1)   menu=0;
            if (ButtonUP.read()==1)     menu--;
            //if (ButtonDOWN.read()==1)   menu++;
            if (ButtonSELECT.read()==1)
                {
                    lcd.cls();
                    lcd.printf("> SOLENOID: ON");
                    lcd.locate(0,1);
                    lcd.printf("           ");
                    Solenoid=1;
                    wait(1);
                    Solenoid=0;
                }
        break;
        case 50:            //tekenen afsluiten
            lcd.cls();
            lcd.printf(" Shutting Down!");
            menu=0;
            
            uitzetten();
   
        break;
        case 55:         
            if (ButtonSTOP.read()==1)       menu=50;
            if (ButtonUP.read()==1)         menu++;
            
            
            aansturing();                                                                                    //aansturing
            
            
        break;
        case 56:
            wait(0.2);
            lcd.cls();
            lcd.printf("  PAUSE       ");
            lcd.locate(0,1);
            lcd.printf("> RESUME");
                       
            Solenoid=1;
            pwm_motor1.write(0);
            pwm_motor2.write(0);
            
            while(ButtonSELECT.read()==1);
            while(menu==56)
            {
            if (ButtonSTOP.read()==1)       menu=50;
            if (ButtonSELECT.read()==1)
            {     
                menu--;
                 lcd.cls();
                 lcd.printf("  Drawing ...");
                 lcd.locate(0,1);
                 lcd.printf("UP: Pause");
            }
            }

                     
            
        break;
    }

            if (menu!=55) wait(0.2);      
    }

    
}

void aansturing(void)
{ 
        while(looptimerflag != true);
        looptimerflag = false;
        
        //uitlezen
        emg_value1 = EMG1.read();
        emg_value2 = EMG2.read();
        emg_value3 = EMG3.read();
        emg_value4 = EMG4.read();
        
        //filtering
        EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
        EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
        EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
        EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
        EMGhp1=abs(EMGhp1);
        EMGhp2=abs(EMGhp2);
        EMGhp3=abs(EMGhp3);
        EMGhp4=abs(EMGhp4);
        EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
        EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
        EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
        EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
        //pc.printf("%.2f   ",emg_value1);
        //pc.printf("%.2f   ",emg_value2);
        //pc.printf("%.2f   ",emg_value3);
        //pc.printf("%.2f   ",emg_value4);
        //pc.printf("%.2f   ",EMGlp1);
        //pc.printf("%.2f   ",EMGlp2);
        //pc.printf("%.2f   ",EMGlp3);
        //pc.printf("%.2f   ",EMGlp4);
        
        //berekenen setpoint
        //hoekinput
        
        if((EMGlp1-EMGmin1)<=0.0)    v1=0.0;       
        else                         v1=(EMGlp1-EMGmin1)/(EMGmax1-EMGmin1);       
        if((EMGlp2-EMGmin2)<=0.0)    v2=0.0;
        else                         v2=(EMGlp2-EMGmin2)/(EMGmax2-EMGmin2);   
        if((EMGlp3-EMGmin3)<=0.0)    v3=0.0;
        else                         v3=(EMGlp3-EMGmin3)/(EMGmax3-EMGmin3);    
        if((EMGlp4-EMGmin4)<=0.0)    v4=0.0;
        else                         v4=(EMGlp4-EMGmin4)/(EMGmax4-EMGmin4);
            
        t_timer=t_timer+LOOPTIME;
        
        pc.printf("%.2f   ",v1);
        pc.printf("%.2f   ",v2);
        pc.printf("%.2f   ",v3);
        pc.printf("%.2f   ",v4);
        if(v1<=0.1 && v2<=0.1 && v3<=0.1 && v4<=0.1) {
            Solenoid=1;             //Pen van papier
            input=0.0;
            snelheid=0.0;
            }  
        else {
            Solenoid=0;             //Pen op papier
            snelheid=drawspeed;
            if(v2>v1) {
            input=(atan((v3-v4)/(v1-v2))+PI);
            }
            else {
            input=(atan((v3-v4)/(v1-v2)));
            }
            }
            pc.printf("%.2f  \n\r",input);
            //snelheid=drawspeed;
            //input=t_timer*0.8+PI;
        
        //snelheidsvector met beperking positie / encoder uitlezen
        M1position = motor1.getPosition();
        M2position = motor2.getPosition();
        //M2phi=M1position-M2position+1600.0;         //phi2 = phi1 - theta + 1600
        
        //Px=cos((M1position/3200.0)*2.0*PI)*ARM1+cos((M2position/3200.0)*2.0*PI)*ARM2;
        //Py=sin((M1position/3200.0)*2.0*PI)*ARM1+sin((M2position/3200.0)*2.0*PI)*ARM2;
        
        //if(Px > 0.29 || Px < 0.125)     vx=0;
        //else                            
        vx=cos(input)*snelheid; 
        //if(Py < -0.425 || Py > -0.125)  vy=0;
        //else                            
        vy=sin(input)*snelheid;
        
        //input positie
        phi1=(motor1.getPosition()/3200.0)*2.0*PI;
        theta=(motor2.getPosition()/3200.0)*2.0*PI;
        phi2=theta+phi1-PI;
        
        //Jacobiaan
        // [a  b]
        // [c  d]
        a=-sin(phi1)*ARM1;
        b=-sin(phi2)*ARM2;
        c=cos(phi1)*ARM1;
        d=cos(phi2)*ARM2;
        
        //inverse
        // [ai  bi]
        // [ci  di]
        ai=d/(a*d-b*c);
        bi=-b/(a*d-b*c);
        ci=-c/(a*d-b*c);
        di=a/(a*d-b*c);
        
        //vermenigvuldiging
        // [ai  bi]   [vx]   [w1]
        // [ci  di] * [vy] = [w2]
        w1=ai*vx+bi*vy; //=wM1 hoeksnelheid van motor 1
        w2=ci*vx+di*vy;
        wM2=w2-w1;//hoeksnelheid motor 2
        
        //Beveiliging hoeksnelheden
        keep_in_range(&w1, -1000,1000);
        keep_in_range(&wM2, -1000,1000);
    
        //Motoraansturing
        //t_sin=t_sin + 0.05; 
        //if (t_sin>=2*PI) t_sin=0.0;
        setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1;          //sin(t_sin)*1600;
        setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2;
        
        //Beperking hoeken
        keep_in_range(&setpointM1, 0,1600);//Heel rondje = 3200 pulsen, Half rondje = 1600 pulsen
        keep_in_range(&setpointM2, 1600,3050);// Begrensd op 20 graden minimaal, werkelijke minimale waarde is 15 graden
        
        foutM1 = setpointM1-M1position;
        foutM2 = setpointM2-M2position;
        foutI1 = foutImin1 + foutM1*LOOPTIME;
        foutI2 = foutImin2 + foutM2*LOOPTIME;
        foutverschilM1 = foutM1-foutmin1M1;
        foutverschilM2 = foutM2-foutmin1M2;
        foutverschilM1 = CLP1*foutverschilmin1M1+CLP2*foutverschilM1;
        foutverschilM2 = CLP1*foutverschilmin1M2+CLP2*foutverschilM2;
        pwm_to_motor1 = foutM1*CP+foutverschilM1*CDloop+foutI1*CI;
        pwm_to_motor2 = foutM2*CP+foutverschilM2*CDloop+foutI2*CI;//foutM2*CP+foutverschilM2*CDloop;
        keep_in_range(&pwm_to_motor1, -0.2,0.2);
        keep_in_range(&pwm_to_motor2, -0.2,0.2);
        
        
        if(pwm_to_motor1 > 0) {
            motordir1 = 1;
            pwm_to_motor1=pwm_to_motor1+0.08;
            }
        else {
            motordir1 = 0;
            pwm_to_motor1=pwm_to_motor1-0.08;
            }
        if(pwm_to_motor2 > 0) {
            motordir2 = 1;
            pwm_to_motor2=pwm_to_motor2+0.08;
            }
        else {
            motordir2 = 0;
            pwm_to_motor2=pwm_to_motor2-0.08;
            }
        
        
        //WRITE VALUE TO MOTOR  
        pwm_motor1.write(abs(pwm_to_motor1));
        pwm_motor2.write(abs(pwm_to_motor2));
        
        //Definieren waarden in de verleden tijd
        foutmin1M1=foutM1;
        foutmin1M2=foutM2;
        foutverschilmin1M1=foutverschilM1;
        foutverschilmin1M2=foutverschilM2;
        foutImin1=foutI1;
        foutImin2=foutI2;
        setpointmin1M1=setpointM1;     
        setpointmin1M2=setpointM2;
        emg_value1min1=emg_value1;
        emg_value2min1=emg_value2;
        emg_value3min1=emg_value3;
        emg_value4min1=emg_value4;
        EMGhp1min1=EMGhp1; 
        EMGhp2min1=EMGhp2;
        EMGhp3min1=EMGhp3;
        EMGhp4min1=EMGhp4;
        EMGlp1min1=EMGlp1;
        EMGlp2min1=EMGlp2;
        EMGlp3min1=EMGlp3;
        EMGlp4min1=EMGlp4;     
    

}
void uitzetten(void)
{
    float BeginM1 = 800;
    float BeginM2 = 2400;
    Solenoid=1;
    while(BeginM1 - motor1.getPosition() >= 10 || BeginM1 - motor1.getPosition() <= -10 || BeginM2 - motor2.getPosition() >= 10 || BeginM2 - motor2.getPosition() <= -10) 
    {
        Ticker looptimer;
        looptimer.attach(setlooptimerflag,LOOPTIME); 
        while(looptimerflag != true);
        looptimerflag = false;
        M1position=motor1.getPosition();
        M2position=motor2.getPosition();
        pc.printf(" %f ",M2position);
        pwm_to_motor1 = (BeginM1 - M1position)*.002;
        pwm_to_motor2 = (BeginM2 - M2position)*.002;
        keep_in_range(&pwm_to_motor1, -0.05,0.05);
        if(pwm_to_motor1 > 0)
            motordir1 = 1;
        else
            motordir1 = 0;
        keep_in_range(&pwm_to_motor2, -0.05,0.05);
        if(pwm_to_motor2 > 0)
            motordir2 = 1;
        else
            motordir2 = 0;
        //WRITE VALUE TO MOTOR
        pwm_motor1.write(abs(pwm_to_motor1));
        pwm_motor2.write(abs(pwm_to_motor2));
        float sent_pwm = abs(pwm_to_motor2);
        pc.printf(" %f ",sent_pwm);
          
    }
             
     pwm_motor1.write(0);
     pwm_motor2.write(0);  
     menu=0; 
}

void keep_in_range(float * in, float min, float max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}

void setlooptimerflag(void)
{
    looptimerflag = true;
}