File content as of revision 25:c97d079e07f3:

#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 ptmrL(A2), ptmrR(A3);      // Intensiteit 'EMG' signaal door potmeter
AnalogIn KS(A4);                    // Killswitch
HIDScope scope(6);                  // 3 scopes
Ticker EMGticker, tickerRegelaar, tickerBtn, 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_EMG1, KALIBRATE_AIM, KALIBRATE_PEND, AIM, REM, FIRE};  // Spelfases, ACKNOWLEDGEMENT: BMT groep 4 2014
uint8_t state = KALIBRATE_EMG1;     // 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;
volatile int emgModeL = 1, emgModeR = 1 potModeL = 1, potModeR = 1; // emg signaal waarden (modes)

const double servoL = 0.001, servoR = 0.0011; // uitwijking servo, bereik tussen L en R  (= pulsewidth pwm servo) 
const double tRegelaar = 0.005, tBtn = 0.005, tRem = 0.1;     // tijd ticker voor regelaar en knoppen/EMG   
const double Fs = 200;                   //Sample frequentie Hz
double t = 1/ Fs;                        // tijd EMGticker
double ymin = 0, ymax = 0;
double thresholdlow = 0, thresholdmid = 0, thresholdhigh= 0;

volatile bool L = false, R = 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 EMGkalibratie(){                       // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
    LedB = 1;
    Init();    
    ymin = KalibratieMin(emgL);
    wait(1);
    ymax = KalibratieMax(emgL);

    // bepalen van thresholds voor aan/uit
    thresholdlow = 8 * ymin; // standaardwaarde
    thresholdmid = 0.5 * ymax; // afhankelijk van max output gebruiker
    thresholdhigh = 0.8 * ymax;

    pc.printf("ymax = %f en ymin = %f \n",ymax, ymin); //bugfix
    }
int EMGfilter(AnalogIn& emg, int w){    
    double u = emg.read();                      // uitlezen emg signaal
    double y = Filterdesigns(u);                // signaal filteren    
    int mode = Mode(y, ymax, thresholdlow, thresholdmid, thresholdhigh); //bepaald welk signaal de motor krijgt (1, 2 ,3 )    
    
    scope.set(w,u);                             //ongefilterde waarde naar scope 1
    scope.set(w+1,y);                            //gefilterde waarde naar scope 2
    scope.set(w+2,emgModeL);
    scope.send();                               //stuur de waardes naar HIDscope
    return mode;
    }
void setEmgFlag(){
    emgFlag = true;
    }   
void btnSetAction(){                // Set knop K64F
    btn = true;                     // GoFlag setknop 
    }  
void setBtnFlag(){                  // Go flag regelaar Aim motor 
    btnFlag = true;
    }
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.5);              // width aanpassen  
    }    
bool controlAim(){                  // Function om aim motor aan te sturen
    bool both = false;              // boolean of beide knoppen ingedrukt zijn       

    if(!(ptmrL.read() >0.3 && ptmrR.read()>0.3) != !(emgModeL!=1 && emgModeR!=1)) {     // Potmeters L en R beide aan XOR EMG beide aangespannen
        R = false;
        L = false;
        pwmM2.write(0);                // Aim motor stilzetten       
        aimState = OFF;        
        }
    else if(!(ptmrL.read() >= 0.8 != !(emgModeL == 3)) && L) {                 // Potmeter L vol ingedrukt XOR emgModeL is 3
        both = true;                   // Return true
        pwmM2.write(0);                // Aim motor stilzetten
        aimState = OFF; 
        L = false;        
        } 
    else if((!(ptmrR.read() > 0.8) != !(emgModeR == 3)) && aimState!=OFF)  {      // Potmeter R vol ingedrukt XOR emgModeR == 3 
        R = false;
        pwmM2.write(0);                // Aim motor stilzetten        
        aimState = OFF; 
        PRINT("Motor freeze\r\n");
        } 
    else if((!(ptmrL.read() > 0.3 && ptmrL.read() < 0.6) != !(emgModeL == 2)) && aimState != CCW && L) {      // Potmeter L half XOR emgModeL ==2 AND richting is niet CCW
        aimState = CCW;                 // draai CCW
        pc.printf("Turn negative (CCW)\r\n");
        motorAim(0);
        } 
    else if((!(ptmrR.read() > 0.3 && ptmrR.read() < 0.6) != !(emgModeR == 2)) && aimState != CW && R) {      // Potmeter R half XOR emgModeR == 2 AND richting is niet CW
        aimState = CW;                  // draai CW
        PRINT("Turn positive (CW)\r\n");
        motorAim(1);
        }
    if(ptmrR.read() < 0.1 && emgModeR == 1 && !R){           // R is niet aan
        R = true;
        }
    if(ptmrL.read() < 0.1 && emgModeL == 1 && !L){           // L is niet aan
        L = true;
        }
    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
    tickerBtn.attach(&setBtnFlag,tBtn);                  // ticker knop controle
    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_EMG1: {
                pc.printf("Kalibrate EMG signal in 5 seconds\r\n");
                lcd.cls();
                lcd.printf("Kalibrate EMG\n in 5 sec.");
                
                EMGkalibratie();
                                   
                state = KALIBRATE_AIM;
                break;
                }
            case KALIBRATE_AIM: {
                pwmM2.period(1/100000);            // aanstuurperiode motor 2
                PRINT("Kalibrate aim motor\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){ 
                    if(regelaarFlag){       // motor regelen op GoFlag
                        regelaarFlag = false;                  
                        checkAim();     // Controleer positie                       
                        }
                        
                    if(emgFlag){            // Go flag EMG sampelen
                        emgFlag = false;
                        emgModeL = EMGfilter(emgL,0);
                        emgModeR = EMGfilter(emgR,3);
                        }        
                    if(btnFlag){                // Go flag button controle?                        
                        btnFlag = 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.5);               // Motor aanzetten, laag vermogen
                btn = false;                        
                while(state==KALIBRATE_PEND){                    
                    if(btnFlag){                        
                        pc.printf("");          // lege regel printen, anders doet setknop het niet
                        btnFlag = false;                    
                        if (btn){               // Als setknop ingedrukt is reset
                            pwmM1.write(0);             // Motor 1 stilzetten
                            enc1.reset();               // encoder 1 resetten
                            pc.printf("Pendulum kalibrated\r\n");
                            btn = false;                // knop op false
                            state = AIM;                // volgende fase                            
                            }
                        }             
                    }
                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;
                        emgModeL = EMGfilter(emgL,0);
                        emgModeR = EMGfilter(emgR,3);
                        }                   
                    if(btnFlag){                // Go flag button controle?                                                
                        btnFlag = 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;
                        emgModeL = EMGfilter(emgL,0);
                        emgModeR = EMGfilter(emgR,3);
                        }
                    if(btnFlag){                    // Go flag button controle?  == rem aansturen                      
                        btnFlag = false;
                                       
                        if((ptmrR.read() < 0.1)) {           // R is niet aan
                            R = true;
                            }
                        if((ptmrL.read() < 0.1)) {           // L is niet aan
                            L = true;
                            }
                        if(L && R){                    
                            if(!(ptmrL.read() >0.3 && ptmrR.read()>0.3) != !(emgModeL!=1 && emgModeR!=1)) {     // beide aangespannenR = false;                                                                                                
                                state = FIRE;
                                }
                            else if(!(ptmrL.read() >= 0.8 != !(emgModeL == 3))) {                 // links vol ingedrukt = schieten                      
                                state = FIRE;
                                } 
                            else if(!(ptmrR.read() > 0.8) != !(emgModeR == 3))  {               //rechts vol ingedrukt = schieten                        
                                state = FIRE;
                                } 
                            else if((!(ptmrL.read() > 0.3 && ptmrL.read() < 0.6) != !(emgModeL == 2)) && L) {      // links ingedrukt = rem verlagen
                                if(remPerc>1){
                                    remPerc--;          // Rem percentage verlagen met 1
                                    }
                                L = false;
                                
                                } 
                            else if((!(ptmrR.read() > 0.3 && ptmrR.read() < 0.6) != !(emgModeR == 2)) && R) {      // Rechts half ingedrukt = rem verhogen
                                if(remPerc<9){
                                    remPerc++;          // rem percentage verhogen met 1
                                    }
                                R = false;                        
                                }               
                            
                            if((ptmrL.read() > 0.3 || ptmrR.read() > 0.3) || (emgModeL != 1 || emgModeR !=1)){               // Print rem scale als een of beide knoppen aan zijn
                                pc.printf("Current breaking scale: %i\r\n", abs(remPerc)); 
                                }
                            }
                        }
                    }
                L = false;
                R = false;                              
                break;
                }                    
            case FIRE: {            
                pc.printf("Shooting\r\n");
                servo.pulsewidth(servoL);       // schrijfrem release
                pwmM1.write(0.5);              // 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;
                        //pc.printf("Slinger encoder: %i\r\n", enc1.getPulses()%4200);                                                                                                 
                        if((abs(enc1.getPulses())+50)%4200 < 150){    // Slinger 1 rondje laten draaien 
                            pwmM1.write(0);                      // motor uitzetten
                            pc.printf("Pendulum on startposition\r\n");
                            state = AIM;                        // Aim is volgende fase
                            }                           
                        }                       
                    }                                                     
                break;
                }
            }
        }
    }