File content as of revision 24:ddd69385b55f:

#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);           // 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 btnL(A2), btnR(A3);        // push buttons, links en rechts
AnalogIn ptmrL(A2), ptmrR(A3);      // Intensiteit 'EMG' signaal door potmeter
AnalogIn KS(A4);                    // Killswitch
Ticker tickerRegelaar, tickerBtn, tickerRem;   // regelaar button en rem ticker
Timer EMGTimer;                     // timer voor EMG meting
// EMG spul:
AnalogIn    emg(A0); //Analog input van emg kabels Links
AnalogIn    emgR(A1); //Analog input van emg kabels Rechts
HIDScope    scope(3); //3 scopes
Ticker      EMGticker;
DigitalOut  LedBlue(LED3);
DigitalIn   button(PTA4);

// QEI Encoder(poort 1, poort 2, ,counts/rev
    QEI enc1 (D13, D12, NC, 64);
    QEI 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 double servoL = 0.001, servoR = 0.0011; // uitwijking servo, bereik tussen L en R  (= pulsewidth pwm servo) 
const int on = 0, off = 1;                    // aan / uit
volatile bool btn = false;                       // boolean om programma te runnen, drukknop ingedrukt
int maxCounts = 2100;                   // max richt motor counts Aim motor
volatile bool regelaarFlag = false, btnFlag = false, remFlag = false;     // Go flags
const double tRegelaar = 0.005, tBtn = 0.005, tRem = 0.1;     // tijd ticker voor regelaar en knoppen/EMG   
int remPerc = 0;
volatile bool L = false, R = false;      // Booleans voor knop link en knop rechts
volatile int emgModeL = 1, emgModeR = 1;   // emg signaal waarden 

//Sample frequentie
double Fs = 200; //Hz
double t = 1/ Fs; // voor EMGticker

bool readymax = 0, readymin = 0;
double ymin, ymax;
double thresholdlow, thresholdmid, thresholdhigh;
bool go_flag = false;

//----------------------------Functions-----------------------------------------------------------------------
void EMGkalibratie(){
    LedBlue = 1;
    Init();    
    ymin = KalibratieMin(readymin);
    wait(1);
    ymax = KalibratieMax(readymax);

    // 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
    }

void EMGfilter(){
    //uitlezen emg signaal
    double u = emg.read();
    double y = Filterdesigns(u);
    //pc.printf("%f \n",y); //bugfix
    // Plotten in HIDscope
    emgModeL = Mode(y, ymax, thresholdlow, thresholdmid, thresholdhigh); //bepaald welk signaal de motor krijgt (aan, uit, middel)
    scope.set(0,u); //ongefilterde waarde naar scope 1
    scope.set(1,y); //gefilterde waarde naar scope 2
    scope.set(2,emgModeL);
    scope.send(); //stuur de waardes naar HIDscope
    }

void Go_flag(){
    go_flag = true;
    }
// -----

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 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(&Go_flag, t);                      // ticker EMG, 500H
    
    lcd.cls();
    lcd.printf("NEW GAME \n HELLO!");
     
    pc.printf("\n\n\n\n\n");
    pc.printf("--- 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(go_flag){            // Go flag EMG sampelen
                        go_flag = false;
                        EMGfilter();
                        }        
                    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(go_flag){            // Go flag EMG sampelen
                        go_flag = false;
                        EMGfilter();
                        }                   
                    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(go_flag){                    // Go flag EMG sampelen
                        go_flag = false;
                        EMGfilter();
                        }
                    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;
                }
            }
        }
    }