File content as of revision 4:377ddd65e4a6:

/********************************************/
/*                                          */
/*   BRONCODE GROEP 7, MODULE 9, 2014       */
/*       *-THE SLAP-*                       */
/*                                          */
/* -Anne ten Dam                            */
/* -Laura de Heus                           */
/* -Moniek Strijdveen                       */
/* -Bart Arendshorst                        */
/* -Peter Bartels                           */
/********************************************/

/*
#include voor alle libraries
*/
#include "TextLCD.h"
#include "mbed.h"
#include "encoder.h"
//#include "HIDScope.h"
#include "PwmOut.h"

/*
#define vaste waarden
*/
/*definieren pinnen Motor 1*/
#define M1_PWM PTA5
#define M1_DIR PTA4
#define M2_PWM PTC8
#define M2_DIR PTC9
/*Definieren minimale waarden PWM per motor*/
#define PWM1_min_50 0.529 /*met koppelstuk!*/
#define PWM2_min_30 0.529 /*aanpassen! Klopt nog niet met koppelstuk*/
/*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
#define TSAMP 0.005
#define K_P (100)
#define K_I (50 * TSAMP)
#define K_D (1)
//#define K_D (0.0005 /TSAMP)
#define I_LIMIT 100.
#define PI 3.1415926535897
#define lengte_arm 0.5

/*
Geef een naam aan een actie en vertel welke pinnen hiervoor gebruikt worden.
*/
TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); // rs, e, d4-d7
Encoder motor1(PTD3,PTD1);
Encoder motor2(PTD5, PTD0); 
PwmOut pwm_motor1(M1_PWM);
PwmOut pwm_motor2(M2_PWM); 
DigitalOut motordir1(M1_DIR);
DigitalOut motordir2(M2_DIR); 
DigitalOut LEDGREEN(LED_GREEN);
DigitalOut LEDRED(LED_RED); 
Serial pc(USBTX,USBRX);
/*
definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde
*/
Ticker statemachine;
Ticker screen;
int previous_herhalingen = 0;
int current_herhalingen = 0;
int PWM2_percentage = 100; 
int aantal_rotaties_1 = 10;
int aantalcr_1 = 1600;
int aantalcr_2 = 960; 
int beginpos_motor1;
int beginpos_motor1_new; 
int beginpos_motor2;
int beginpos_motor2_new;
int previous_pos_motor1 = 0;
int current_pos_motor1;
int delta_pos_motor1_puls;
void clamp(float * in, float min, float max);
volatile bool looptimerflag;
int16_t gewenste_snelheid = 2;
int16_t gewenste_snelheid_rad = 4; 
int16_t gewenste_snelheid_rad_return = 1;
float pid(float setpoint, float measurement);
float pos_motor1_rad;
float PWM1_percentage = 0;
float delta_pos_motor1_rad;
float snelheid_motor1_rad;
float snelheid_arm_ms; 
float PWM1; 
float PWM2;
float Speed_motor1;
float Speed_motor1rad;
float setpoint = 0;
float prev_setpoint = 0; 

//HIDScope scope(6);

enum  state_t {RUST, ARM_KALIBRATIE, EMG_KALIBRATIE, METEN_RICHTING, METEN_HOOGTE, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES
state_t state = RUST;

//functies die vanuit de statemachinefunction aangeroepen kunnen worden
void rust() {
    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
}
    
void arm_kalibratie() {
    //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
    motor1.setPosition(0);
    motor2.setPosition(0);    
}

void emg_kalibratie() {
    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
}

void meten_richting() {
    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
}

void meten_hoogte() {
    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
}

void instellen_richting() {
    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
}

void GotoPosition (float position_setpoint_rad, float speed_radpersecond) {
    current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
    delta_pos_motor1_puls = current_pos_motor1 - previous_pos_motor1; //Bereken wat het verschil in waarde is voor het berekenen van de snelheid(puls)
    pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
    delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad)
    //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen
    //snelheid_arm_ms = snelheid_motor1_rad * lengte_arm; //deze waarde maar lengte van de arm om de snelheid van het uiteinde van die arm te verkrijgen
    //scope.set(0, snelheid_motor1_rad);
    
    previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
    
    //nu gaan we positie regelen i.p.v. snelheid.
    pc.printf("%f\n\r",pos_motor1_rad - position_setpoint_rad);
    if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.2) //if position error < ...rad, then stop.
    {
        speed_radpersecond = 0; 
        setpoint = pos_motor1_rad;
        current_herhalingen = previous_herhalingen + 1; 
        previous_herhalingen = current_herhalingen;
        pc.printf("stop\n\r");
    }
    else if(pos_motor1_rad - position_setpoint_rad < 0)
    {    
        setpoint = prev_setpoint +( TSAMP * speed_radpersecond);       
    }
    else
    {
        setpoint = prev_setpoint -( TSAMP * speed_radpersecond);       
    }
    /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
    
    PWM1_percentage = pid(setpoint, pos_motor1_rad);
    //scope.set(1, setpoint-pos_motor1_rad); 
    
    if (PWM1_percentage < -100)
    {
        PWM1_percentage = -100;
    }
    else if (PWM1_percentage >100)
    {
        PWM1_percentage =100;
    }
    
    if(PWM1_percentage < 0)
    {
        motordir1 = 1;
    }
    else
    {
        motordir1 = 0;
    }
        
    pwm_motor1.write(abs(PWM1_percentage/100.));//abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
    //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
    prev_setpoint = setpoint;
    //scope.send();  
}



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;
    //scope.set(2, out_p);
    //scope.set(3, out_i);
    //scope.set(4, out_d);
    return out_p + out_i + out_d;
}

void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variebele instaat). Dus je slaat niet de variabele op
// maar de locatie van de variabele. 
{
    *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c 
    // *in = het getal dat staat op locatie van in --> waarde van new_pwm
}
    
void statemachinefunction()
{
    switch(state) {
        case RUST: {
            rust();
            /*voorwaarde wanneer hij door kan naar de volgende case*/
            if (current_herhalingen == 600) 
            {
                current_herhalingen = 0;
                previous_herhalingen = 0;
                state = ARM_KALIBRATIE;
            }
            break;
            /*bepalen hoe je zorgt dat je maar 1x de kalibraties uitvoerd! (Of eventueel de arm kalibratie elke keer!!)*/
            //if (metingstatus<5);
            //    state = ARMKALIBRATIE;
            //if (metingstatus==5);
            //    state = METEN_RICHTING;
            //break;
            //}
        }

        case ARM_KALIBRATIE: 
        {
            arm_kalibratie();
            if (current_herhalingen == 200) 
            {
                current_herhalingen = 0;
                previous_herhalingen = 0;
                state = EMG_KALIBRATIE;
                motor1.setPosition(0);
                motor2.setPosition(0);
                pwm_motor1.period_us(100);
                pwm_motor2.period_us(100);
            }
            break;
        }

        case EMG_KALIBRATIE: 
        {
            emg_kalibratie();
            if (current_herhalingen == 200) 
            {
                state = METEN_RICHTING;
                current_herhalingen = 0;
                previous_herhalingen = 0;
            }
            break;
        }

        case METEN_RICHTING: 
        {
            meten_richting();
            if (current_herhalingen == 200) 
            {
                state = METEN_HOOGTE;
                current_herhalingen = 0;
                previous_herhalingen = 0;
            }
            break;
        }

        case METEN_HOOGTE: 
        {
            meten_hoogte();
            if (current_herhalingen == 200) 
            {
                state = INSTELLEN_RICHTING;
                current_herhalingen = 0;
                previous_herhalingen = 0;
            }
            break;
        }

        case INSTELLEN_RICHTING: 
        {
            instellen_richting();
            if (current_herhalingen == 200) 
            {
                state = SLAAN;
                current_herhalingen = 0;
                previous_herhalingen = 0;
            }
        break; 
            
        }

        case SLAAN: 
        {
            GotoPosition(0.7,4);
            if (current_herhalingen == 200) 
            {
                state = RETURN2RUST;
                current_herhalingen = 0;
                previous_herhalingen = 0;
                prev_setpoint =0; 
                setpoint =0;
                
            }
        break;    
        }

        case RETURN2RUST: 
        {
            GotoPosition(0,1);
            /*if (current_herhalingen == 200) 
            {
                state = RUST;
                current_herhalingen = 0;
                previous_herhalingen = 0;
            }
            */
            break;
        }
        
        default: {
            state = RUST;
        }

    }//switch(state)
}//void statemachinefunction


void screenupdate(){
    if(state==RUST){
        lcd.cls(); 
        lcd.locate(0,0);
        lcd.printf("S.T.I.E.N.E.N.");   //regel 1 LCD scherm
        lcd.locate(0,1);
        lcd.printf("  GROEP 7   ");
    }
    else{     
        lcd.cls();
        lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.
    }
}

int main() {
    pc.baud(115200);
    statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
    screen.attach(&screenupdate, 0.2);
    while(1);
}