File content as of revision 18:1188cdb7ccbe:

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

#define TSAMP 0.01
#define K_P (0.01)
#define K_I (0  *TSAMP)
#define K_D (0  /TSAMP)
#define I_LIMIT 1.
#define POT_AVG 50
#define WACHTEN 1
#define SLAAN 2
#define TERUGKEREN 3
#define ANGLEMAX 244
#define ANGLEMIN 0

MODSERIAL pc(USBTX,USBRX);
void clamp(float * in, float min, float max);
float pid(float setpoint, float measurement);
AnalogIn potmeter(PTC2);
volatile bool looptimerflag;
float potsamples[POT_AVG];
float EMG(float biceps, float triceps) // waarschijnlijk niet float omdat input functies zijn

void setlooptimerflag(void)
{
    looptimerflag = true;
}

int main()
{
    float v3=0.2,v2=0.1,v1=0.05, readbiceps=0, readtriceps=0
    int ANGLEMAX, ANGLEMIN
    // zie schrift voor berekeningen ANGLEMAX. Is float ANGLEMAX nog nodig als ANGLEMAX = defined
    Encoder motor1(PTD0,PTC9);
    PwmOut pwm_motor(PTA5);
    pwm_motor.period_us(100);
    DigitalOut motordir(PTD1);
    
    int toestand = TERUGKEREN;
    //moet hier wel int voor staan? want terugkeren is al gedifined alszijnde 3
    
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,TSAMP);
    while(1) {
        float setpoint;
        float new_pwm;
        while(!looptimerflag);
        looptimerflag = false;
        
        if (motor1.getPosition()<ANGLEMIN && toestand = TERUGKEREN)
        {
            toestand = WACHTEN;
            if (EMG(readbiceps, readtriceps) != 0)
            {
                toestand = SLAAN;
               // potmeter.read() wordt waarschijnlijk output EMG functie (0,1,2,3)
                if ( EMG(readbiceps, readtriceps)=3 && motor1.getPosition()<ANGLEMAX) //angle<ANGLEMAX moet ergens anders staan (iets met toestand)
                {
                    setpoint = v3; //experimenteel bepaald, snelheid(?) om bovenste goal te raken 
                }
                else if ( EMG(readbiceps, readtriceps)=2)
                {
                    setpoint = v2; //experimenteel bepaald, snelheid om middelste goal te raken
                }
                else if (EMG(readbiceps, readtriceps)=1)
                {
                    setpoint = v1; //experimenteel bepaald, snelheid om bovenste goal te raken
                }
                else //waarschijnlijk niet nodig want hij kan hier niet komen als readEMG nul is
                {
                    setpoint = 0;  // toestand = TERGUGKEREN?
                }
            }
        }
        
    // De toestand moet vanaf wacht naar slaan gaan bij aanspanning van spier(1) Hier zit waarschijnlijk een !!!delay!!! in 
    // vanwege het middelen over de tijd van het signaal. van MK en Tanja stukje script met functie met return (0/1/2 of 3).
    // angle = ???? Encoder =motor1.getPosition()
    //----------------------------------------------------------------------new_pwm = getal, opletten dat deze tijdens het slaan niet kan veranderen!
        new_pwm = pid(setpoint, motor1.getPosition());
        clamp(&new_pwm, -1,1);
    //-------------------------------------------------------------------------------------output motor richting-------------------------------------
        if(new_pwm > 0)
            motordir = 0;
        else
            motordir = 1;
        pwm_motor.write(abs(new_pwm));
        if(motor1.getPosition()>ANGLEMAX && toestand=SLAAN)
        { 
            toestand = TERUGKEREN;
            // pc.printf ("Toestand = terugkeren!");
        }
    }
    
}

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


float pid(float setpoint, float measurement)
{
    float error;
    static float prev_error = 0;
    float           out_p = 0;
    static float    out_i = 0;
    float           out_d = 0;
    error  = setpoint-measurement;
    out_p  = error*K_P;
    out_i += error*K_I;
    out_d  = (error-prev_error)*K_D;
    clamp(&out_i,-I_LIMIT,I_LIMIT);
    prev_error = error;
    return out_p + out_i + out_d;
}