File content as of revision 27:f62e450bb411:

#include "mbed.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "TextLCD.h"
#include "HIDScope.h"
#include "Filterdesigns.h"
#include "Kalibratie.h"
#include "Mode.h"
#include <math.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);                  // 3 scopes
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;

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);
    wait(1);
    double ymax = KalibratieMax(emgL);
    
    if((ymax-ymin) < 0.05){                 // voor als er geen kabels in de EMG zitten
        ymin = 10;
        ymax = 10;
        }    
    thresholdlowL = 8 * 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);
    wait(1);
    double ymax = KalibratieMax(emgR);
    
    if((ymax-ymin) < 0.05){                 // voor als er geen kabels in de EMG zitten
        ymin = 10;
        ymax = 10;
        }    
    // bepalen van thresholds voor aan/uit
    thresholdlowR = 8 * 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
    double y = Filterdesigns(u);                // signaal filteren
    int mode = 1;
    if(side){                                   // linker EMG  
        mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); //bepaald welk signaal de motor krijgt (1, 2 ,3 ) 
        }
    else {                                      // 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.5);              // 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 == 1 && !R){                               // R is niet aan
        R = true;        
        }    
    if(modeL == 1 && !L){                               // L is niet aan
        L = true;
        }
            
    if(modeL>1 && modeR >1) {                           // mode L en mode R beide > 1 
        R = false;
        L = false;
        pwmM2.write(0);                                 // Aim motor stilzetten       
        aimState = OFF;        
        }
    else if((modeL == 3)) {                 
        both = true;                                    // Return true
        pwmM2.write(0);                                 // Aim motor stilzetten
        aimState = OFF; 
        L = false;        
        } 
    else if(modeR == 3 && aimState!=OFF)  {      
        R = false;        
        pwmM2.write(0);                                 // Aim motor stilzetten        
        aimState = OFF; 
        PRINT("Motor freeze\r\n");
        } 
    else if((modeL == 2) && (aimState != CCW) && L) {       // modeL ==2 AND richting is niet CCW
        aimState = CCW;                                 // draai CCW
        pc.printf("Turn negative (CCW)\r\n");
        motorAim(0);
        } 
    else if((modeR == 2) && (aimState != CW) && R) {      // modeR == 2 AND richting is niet CW
        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("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;                                                                        
                        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(regelaarFlag){                        
                        pc.printf("");          // lege regel printen, anders doet setknop het niet
                        regelaarFlag = false;                    
                        if (btn){               // 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                            
                            }
                        }             
                    }
                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;
                                }
                            else if(modeL > 2 || modeR > 2) {// links of rechts vol ingedrukt = schieten                      
                                state = FIRE;
                                }                            
                            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.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;                                                                                                                        
                        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;
                }
            }
        }
    }