File content as of revision 32:40691708c68c:

#include "mbed.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "TextLCD.h"
#include "HIDScope.h"
#include "Filterdesigns.h"
#include "Kalibratie.h"
#include "Mode.h"
//--------------------Classes------------------------
InterruptIn btnSet(PTC6);           // kalibreer knop
DigitalOut ledR(LED_RED), LedB(LED3); // Led op moederbord
MODSERIAL pc(USBTX, USBRX);         // Modserial voor Putty
TextLCD lcd(PTC5, PTC7, PTC0, PTC9, PTC8, PTC1); // LCD scherm op binnenste rij van K64F, rs, e, d4-d7
PwmOut servo(D3);                   // PwmOut servo
AnalogIn emgL(A0), emgR(A1);        // Analog input van emg kabels links en rechts
AnalogIn potL(A2), potR(A3);        // Intensiteit 'EMG' signaal door potmeter
AnalogIn KS(A4);                    // Killswitch
HIDScope scope(6);                  
Ticker EMGticker, tickerRegelaar, tickerRem;   // regelaar button en rem ticker

// QEI Encoder(poort 1, poort 2, ,counts/rev
    QEI enc1 (D13, D12, NC, 64), enc2 (D11, D10, NC, 64);
// Motor1 met PWM aansturen en richting aangeven
    PwmOut pwmM1(D6);       
    DigitalOut dirM1(D7);
// Motor2 met PWM aansturen en richting aangeven
    PwmOut pwmM2(D5);       
    DigitalOut dirM2(D4);
enum spelfase {KALIBRATE_EMG, KALIBRATE_AIM, KALIBRATE_PEND, AIM, REM, FIRE};  // Spelfases, ACKNOWLEDGEMENT: BMT groep 4 2014
uint8_t state = KALIBRATE_EMG;     // eerste spelfase 
enum aimFase {OFF, CW, CCW};         // Aim motor mogelijkheden
uint8_t aimState = OFF;              // mogelijkheid begin
//-------------------------------Variables--------------------------------------------------------------------- 
const int on = 0, off = 1;               // aan / uit
int maxCounts = 2100;                    // max richt motor counts Aim motor
int remPerc = 0;

const double servoL = 0.001, servoR = 0.0011; // uitwijking servo, bereik tussen L en R  (= pulsewidth pwm servo) 
const double tRegelaar = 0.005, tRem = 0.1;     // tijd ticker voor regelaar en knoppen/EMG   
const double Fs = 50;                   //Sample frequentie Hz
double t = 1/ Fs;                        // tijd EMGticker
double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0;
double yL = 0, yR = 0;                  // y waarden EMG links en rechts

volatile bool L = false, R = false, RH = false;      // Booleans voor knop link en knop rechts
volatile bool btn = false;               // boolean om programma te runnen, drukknop ingedrukt
volatile bool regelaarFlag = false, btnFlag = false, remFlag = false;     // Go flags
volatile bool emgFlag = false;
//----------------------------Functions-----------------------------------------------------------------------
void flipLed(DigitalOut& led){              //function to flip one LED   
    led.write(on);
    wait(0.2);
    led.write(off);
    }
void PRINT(const char* text){                // putty en lcd print
    lcd.cls();                              // clear LCD scherm
    lcd.printf(text);                       // print text op lcd
    pc.printf(text);                        // print text op terminal
    }
void EMGkalibratieL(){                       // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
    LedB = 1;
    Init();    
    double ymin = KalibratieMin(emgL, true);
    wait(1);
    double ymax = KalibratieMax(emgL, true);
    
    if((ymax-ymin) < 0.05){                 // voor als er geen kabels in de EMG zitten
        ymin = 10;
        ymax = 10;
        }    
    thresholdlowL = 4 * ymin; // standaardwaarde
    thresholdmidL = 0.5 * ymax; // afhankelijk van max output gebruiker
    thresholdhighL = 0.8 * ymax;

    pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin); //bugfix
    }
void EMGkalibratieR(){                       // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
    LedB = 1;
    Init();    
    double ymin = KalibratieMin(emgR, false);
    wait(1);
    double ymax = KalibratieMax(emgR, false);
    
    if((ymax-ymin) < 0.05){                 // voor als er geen kabels in de EMG zitten
        ymin = 10;
        ymax = 10;
        }        
    thresholdlowR = 4 * ymin; // standaardwaarde
    thresholdmidR = 0.5 * ymax; // afhankelijk van max output gebruiker
    thresholdhighR = 0.8 * ymax;

    pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); //bugfix
    }
int EMGfilter(AnalogIn& emg, bool side){    
    double u = emg.read();                      // uitlezen emg signaal (linker of rechter EMG)    
    int mode = 1;
    if(side){
        double y = FilterdesignsLeft(u);                // signaal filteren                                   // linker EMG  
        mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); //bepaald welk signaal de motor krijgt (1, 2 ,3 ) 
        }
    else {
        double y = FilterdesignsRight(u);                // signaal filteren                                   // rechter EMG
        mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); //bepaald welk signaal de motor krijgt (1, 2 ,3 ) 
        }          
    return mode;
    }    
int PotReader(AnalogIn& pot){                   // potmeter uitlezen en mode bepalen (thresholds)
    double volt = pot.read();
    int mode = 1;    
    if(volt > 0.8){
        mode = 3;
        }
    else if(volt>0.35 && volt<0.65){
        mode = 2;
        }    
    return mode;    
    }    
int defMode(AnalogIn& emg, AnalogIn& pot, bool side){       // bepaal mode uit emg en pot
    int emgMode = EMGfilter(emg, side);
    int potMode = PotReader(pot);
    int mode = 1;    
    if(!(emgMode==1) != !(potMode==1)){         // emgMode = 1 XOR potMode = 1
        if(emgMode > potMode){                 // maximum van emg en pot
            mode = emgMode;
            }
        else{
            mode = potMode;
            }
        }    
    return mode;       
    }        
void setEmgFlag(){
    emgFlag = true;
    }   
void btnSetAction(){                // Set knop K64F
    btn = true;                     // GoFlag setknop 
    } 
void setRegelaarFlag(){             // Go flag button controle    
    regelaarFlag = true;
    }
void setRemFlag(){                  // Go flag rem 
    remFlag = true;
    }  
void checkAim(){                    // controleer of killer switch geraakt is en of max aantal counts niet bereikt is 
    /*double volt = KS.read();    
    if(volt> 0.5 /*|| abs(enc2.getPulses()) > maxCounts*///){
        /*pwmM2.write(0);                                 // Aim motor stilzetten
        pc.printf("BOEM! CRASH! KASTUK! \r\n");        
        }*/
    }
void motorAim(int dir){            // Aim motor laten draaien met gegeven direction 
    dirM2.write(dir);              // richting
    pwmM2.write(0.25);              // width aanpassen  
    }    
bool controlAim(){                  // Function om aim motor aan te sturen
    bool both = false;              // boolean of beide knoppen ingedrukt zijn   
       
    int modeL = defMode(emgL, potL, true);
    int modeR = defMode(emgR, potR, false);    
    
    scope.set(0, modeL);
    scope.set(1, modeR);    
    scope.send();                               //stuur de waardes naar HIDscope
    
    if(modeR < 2 && !R){                        // controleer of mode 1 is geweest
        R = true;        
        }
    if(modeL < 2 && !L){
        L = true;        
        }
                
    if((modeL>2) && (modeR >2 && R && L)) {                       // mode L en mode R beide > 2 
        both = true;                                    // Return true
        pwmM2.write(0);                                 // Aim motor stilzetten
        aimState = OFF;        
        }    
    else if((modeR == 2) && (modeL == 2)) {              
        if(aimState!=OFF){
            pwmM2.write(0);                                 // Aim motor stilzetten        
            aimState = OFF; 
            PRINT("Motor freeze\r\n");
            L = false;
            R = false;
            }
        } 
    else if((modeL == 2) && (aimState != CCW && (modeR == 1))) {        // modeL ==2 AND richting is niet CCW AND modeR = 1
        if(L){                    
            aimState = CCW;                                 // draai CCW
            pc.printf("Turn negative (CCW)\r\n");
            motorAim(0);
            }
        } 
    else if((modeR == 2) && (aimState != CW && (modeL == 1))) {         // modeR == 2 AND richting is niet CW AND modeL = 1
        if(R){
            aimState = CW;                                  // draai CW
            PRINT("Turn positive (CW)\r\n");
            motorAim(1);
            }
        }    
    return both;            
    }               
int main(){
    flipLed(ledR);                                      // test of code begint    
    btnSet.mode(PullUp);                                // Button mode
    btnSet.rise(&btnSetAction);                         // Setknop aan functie koppellen    
    tickerRegelaar.attach(&setRegelaarFlag,tRegelaar);  // ticker regelaar motor    
    tickerRem.attach(&setRemFlag,tRem);                 // ticker rem      
    EMGticker.attach(&setEmgFlag, t);                   // ticker EMG, 500H
        
    pc.printf("\n\n\n\n\n");
    PRINT("--- NEW GAME ---\r\n");    
    while(1){                                           // Programma door laten lopen
        switch(state){
            case KALIBRATE_EMG: {
                PRINT("Kalibrate EMG left in 5 seconds\r\n");
                                
                EMGkalibratieL();
                
                PRINT("Kalibrate EMG right in 5 seconds\r\n");
                EMGkalibratieR();                
                                   
                state = KALIBRATE_AIM;
                break;
                }
            case KALIBRATE_AIM: {
                pwmM2.period(1/100000);            // aanstuurperiode motor 2
                PRINT("Position is kalibrating\r\n");
                wait(1);
                //pc.printf("Use L and R to move pendulum to equilibruim position\r\n L+R = OK\r\n");                                
                while(state==KALIBRATE_AIM){
                    dirM2.write(0);              // richting
                    pwmM2.write(0.25);             // width aanpassen
                    
                    if(KS.read() > 0.5){
                        pwmM2.write(0);             // motor stilzetten
                        enc2.reset();               // resetknop = encoder 1 resetten
                        PRINT("Position kalibrated\r\n");
                        state = KALIBRATE_PEND;         // volgende state
                        }
                        
                    
                    /* 
                    if(regelaarFlag){       // motor regelen op GoFlag
                        regelaarFlag = false;                  
                        checkAim();         // Controleer positie                       
                        }                        
                    if(emgFlag){            // Go flag EMG sampelen
                        emgFlag = false;                                                                        
                        if(controlAim()){           // Buttons met control, if true = beide knoppen = bevestigen
                            enc2.reset();           // resetknop = encoder 1 resetten                        
                            pc.printf("Positie gekalibreerd, kalibreer nu slinger, encoder counts: %i\r\n", enc2.getPulses());
                            state = KALIBRATE_PEND;         // volgende state                        
                            }
                        }*/
                    }
                break;
                }            
            case KALIBRATE_PEND: {
                pc.printf("Kalibrate pendulum motor with setknop\r\n");            
                pwmM1.period(1/100000);         // aanstuurperiode motor 1      
                servo.period(0.02);             // 20ms period aanstuurperiode
                pwmM1.write(0.25);               // Motor aanzetten, laag vermogen
                btn = false;                        
                while(state==KALIBRATE_PEND){                    
                    if(emgFlag){                        
                        pc.printf("");          // lege regel printen, anders doet setknop het niet
                        emgFlag = false;
                        
                        int modeL = defMode(emgL, potL, true);
                        int modeR = defMode(emgR, potR, false);
                                                                                                                   
                        if (btn || (modeL == 3) || (modeR == 3)){               // Als setknop ingedrukt is reset
                            pwmM1.write(0);             // Motor 1 stilzetten
                            enc1.reset();               // encoder 1 resetten
                            PRINT("Pendulum kalibrated\r\n");
                            btn = false;                // knop op false
                            state = AIM;                // volgende fase                            
                            }
                        else if(modeL == 2){
                            pwmM1.write(0);             // Pendulum stilzetten
                            PRINT("Pendulum off\r\n");
                            }
                        else if(modeL == 3){
                            pwmM1.write(0.025);         // Pendulum aanzetten
                            PRINT("Pendulum on\r\n");
                            }
                        }             
                    }
                break;
                }
            case AIM: {                  
                pc.printf("Aim with EMG\r\n");            
                while(state == AIM){               
                    if(regelaarFlag){       // motor regelen op GoFlag
                        regelaarFlag = false;                  
                        checkAim();     // Controleer positie                       
                        }
                    if(emgFlag){            // Go flag EMG sampelen
                        emgFlag = false;                                                            
                        if(controlAim()){                                                                   
                            pc.printf("Position choosen, adjust shoot velocity\r\n");                            
                            state = REM;         // volgende state (REM)                       
                            }                      
                        }
                    }            
                break;
                }
            case REM: {                
                pc.printf("Set break percentage, current is: %.0f\r\n", remPerc);
                L = false;
                R = false;
                while(state == REM){                      
                    if(emgFlag){            // Go flag EMG sampelen
                        emgFlag = false;                        
                        int modeL = defMode(emgL, potL, true);
                        int modeR = defMode(emgR, potR, false);
                                                                                  
                        if(modeR < 2) {           // R is niet aan
                            R = true;
                            }
                        if(modeL < 2) {           // L is niet aan
                            L = true;
                            }
                        if(L && R){                    
                            if((modeL > 1) && modeR > 1) {     // beide aangespannenR = false;                                                                                                
                                state = FIRE;
                                R = false;
                                L = false;
                                }
                            else if(modeL > 2 || modeR > 2) {// links of rechts vol ingedrukt = schieten                      
                                state = FIRE;
                                R = false;
                                L = false;
                                }                            
                            else if((modeL == 2) && L) {      // links ingedrukt = rem verlagen
                                if(remPerc>1){
                                    remPerc--;              // Rem percentage verlagen met 1
                                    }
                                L = false;                                
                                } 
                            else if((modeR == 2) && R) {      // Rechts half ingedrukt = rem verhogen
                                if(remPerc<10){
                                    remPerc++;              // rem percentage verhogen met 1
                                    }
                                R = false;                        
                                }                         
                            if(modeL > 1 || modeR > 1){               // Print rem scale als een of beide knoppen aan zijn
                                pc.printf("Current breaking scale: %i\r\n", remPerc); 
                                }                     
                            }                    
                        }
                    }
                L = false;
                R = false;                              
                break;
                }                    
            case FIRE: {            
                pc.printf("Shooting\r\n");
                servo.pulsewidth(servoL);       // schrijfrem release
                pwmM1.write(0.25);              // motor aanzetten               
                bool servoPos = true;                                                
                while(state == FIRE){           // Zolang in FIRE state
                    if(remFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(remPerc)/10)){             // Rem goFlag en half rondje = remmen
                        remFlag = false;
                        if(servoPos){                       // 
                            servoPos = false;
                            servo.pulsewidth(servoL);       // links   
                            }
                        else{
                            servoPos = true;
                            servo.pulsewidth(servoR);       // rechts
                            }            
                        }                                    
                    if(regelaarFlag){
                        regelaarFlag = false;                                                                                                                        
                        if((abs(enc1.getPulses())+50)%4200 < 150){    // Slinger 1 rondje laten draaien 
                            pwmM1.write(0);                      // motor uitzetten
                            PRINT("Pendulum on startposition\r\n");
                            state = AIM;                        // Aim is volgende fase
                            }                           
                        }                       
                    }                                                     
                break;
                }
            }
        }
    }