File content as of revision 31:113f630f7e7d:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "biquadFilter.h"
#include "encoder.h"

MODSERIAL   pc(USBTX,USBRX);

    //  (DEBUGGING AND TESTING Tools) (0 when pressed and 1 when not pressed)   //
DigitalIn buttonL1(PTC6);                // Button 1 (laag op board) for testing at the lower board
DigitalIn buttonL2(PTA4);                // Button 2 (laag op board) for testing at the lower board
DigitalIn buttonH1(D2);                  // Button 3 (hoog op board) for testing at the top board
DigitalIn buttonH2(D6);                  // Button 4 (hoog op board) for testing at the top board

DigitalOut  debug_led_red(LED_RED);             // Debug LED
DigitalOut  debug_led_green(LED_GREEN);         // Debug LED 
DigitalOut  debug_led_blue(LED_BLUE);           // Debug LED 

HIDScope    scope(2); // HIDSCOPE declared
Ticker Hidscope_measure;

AnalogIn potmeter1(A0); // POTMETER -> DOEN ALSOF EMG SIGNAAL
AnalogIn potmeter2(A1); // POTMETER -> DOEN ALSOF EMG SIGNAAL

double max_L_EMG = 100;
    //  Defining constants   //
volatile bool send_flag;
const double cw=0;                      // zero is clockwise (front view)
const double ccw=1;                     // one is counterclockwise (front view)
const double off=1;                     //Led off
const double on=0;                      //Led on
const double sample_time=0.005;         // tijd voor een sample (200Hz)
  
  
    //  Functions used (described after main)    //
void send(void);
void measure_Hidscope(void);
void deactivate_PID_Controller_strike(void);
void activate_PID_Controller_strike(void);


    //  MAIN function    //
    
int main() {
    QEI motor_turn(D12,D13,NC,32);            // Encoder for motor Turn
    PwmOut pwm_motor_strike(D5);              // Pwm for motor Turn
    DigitalOut motordirection_strike(D4);     // Direction of the motor Turn
    
    double reference_turn=0;                  // Set constant to store reference value of the Turn motor
    double position_turn;                     // Set constant to store current position of the Turn motor
    double position_strike;
    float EMG_R;
    float EMG_L;
    double n;
    debug_led_red=off;
    debug_led_blue=off;
    debug_led_green=off;
    double max_L_EMG;
    double min_L_EMG;
    double max_R_EMG;
    double min_R_EMG;
    double Hit;
    
    //  START OF CODE    //
    pc.printf(" Start of code \n\r");
    
    pc.baud(115200);                                    // Set the baudrate
    
    //  Calibratie   //
        max_L_EMG = 100;                         // Calibreren (max average over 5 seconde?) gemeten integraal EMG over tijd / (tijdsample stappen)=100
        min_L_EMG = 0;
        max_R_EMG = 100;                         // Calibreren
        min_R_EMG = 0;
        Hit=60;                                  // position when bottle is hit
        

        const float Threshold_Bicep_Left_1=((max_L_EMG-min_L_EMG)*0.2)+min_L_EMG;;      //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
        const float Threshold_Bicep_Left_2=((max_L_EMG-min_L_EMG)*0.6)+min_L_EMG;      //(waarde waarop het gemeten EMG signaal 60% van max het maximale is); 
        const float Threshold_Bicep_Right_1=((max_R_EMG-min_R_EMG)*0.2)+min_R_EMG;     //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
        const float Threshold_Bicep_Right_2=((max_R_EMG-min_R_EMG)*0.6)+min_R_EMG;     //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
        const float change_one_bottle=45; //(45 graden change)
        
        pc.printf("left 1: %f left 2: %f right 1: %f right 2: %f  \n\r", Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2);
    
    // Tickers  // 
    Hidscope_measure.attach(send, sample_time);       // GAAT NOG NIET GOED WAARSCHIJNLIJK ALS EEN WHILE LOOP WORDT UTIGEVOERD
    
    pc.printf("wait \n\r");
    wait (3);                                                                          // Wait before starting system
    pc.printf("start control \n\r");
    
    //INFINITE LOOP //
    while(1) 
        {                                                                              // Start while loop  (ACTION loop)    
        // INPUT Filtered EMG SIGNAAL
        
        Nieuwe_actie:                                                                  // start here again when action has finished
        debug_led_red=off;
        debug_led_blue=off;
        debug_led_green=on;                                                            // LED Turns green if ready for a new action
        wait(1);
        EMG_R = (potmeter1.read());                                                //EMG Right bicep (tussen nul en 100%)
        EMG_L = (potmeter2.read());                                                // EMG Left bicep  (tussen nul en 100%)
        pc.printf("EMG_L = %f EMG_R = %f \n\r", EMG_L, EMG_R);
        
        
        // SLAG                                                                        // (No LED -> 0.55s -> red on (turns on every time a new pwm is given) -> finish)
        if (EMG_L > Threshold_Bicep_Left_1 && EMG_R > Threshold_Bicep_Right_1)
            {
                
            debug_led_green=off;
            n=0;
            pc.printf("slag \n\r");
            wait(0.5);                                         
            
            while(EMG_L > Threshold_Bicep_Left_1 && EMG_R > Threshold_Bicep_Right_1)   // Threshold statement still true after 0.5 seconds?
                {
                    if (n==0)  //wordt maar 1 keer uitgevoerd
                        {
                            deactivate_PID_Controller_strike();                        // function that sets P, I and D gain values to zero
                            n=1;
                        }     
                    debug_led_red=off;
                    wait(0.10);                                                        // wait 20 samples
                    debug_led_red=on;
                    pwm_motor_strike=((EMG_L-min_L_EMG)+(EMG_R-min_R_EMG))/((max_L_EMG-min_L_EMG)+(max_R_EMG-min_R_EMG))*0.7 + 0.3;     // min speed 0.3 en max 1
                    wait(0.10);                                                        // wait 20 samples more (pwm changes per 0.1 seconds)
                    motordirection_strike=cw;                                          // towards bottle
                    
                    if((position_strike-Hit)<2)                                        // bottle is hit when position-hit <2 defrees
                        {
                        activate_PID_Controller_strike();                              // function sets back P, I and D gain values
                        pc.printf("einde \n\r");
                        goto Nieuwe_actie;                                             // Finished: Get Ready for new Action control
                        }
                }
            }
                
        activate_PID_Controller_strike();                                              // function sets back P, I and D gain values 
        
        debug_led_red=off;                                                             // not inside an action loop (green light)
        debug_led_blue=off;                                                            
        debug_led_green=on;                                                            // want het kan zijn dat bovenste script half wordt uitgevoerd en dan moet het slag mechanisme wel weer terug
        
        //  DRAAI LINKS     //                                                         // (blue->blue off (very short/visible?)-> red<->blue<-> red -> klaar)
        if (EMG_L > Threshold_Bicep_Left_2 && EMG_R < Threshold_Bicep_Right_1)
            {  
                debug_led_green=off;                                                   // Executing action
                n=0;
                pc.printf("links \n\r");
                while(EMG_L > Threshold_Bicep_Left_1 && EMG_R < Threshold_Bicep_Right_1) 
                    {
                        debug_led_blue=on;
                        if (n==0)                                                      //wordt maar 1 keer uitgevoerd
                        {
                            debug_led_blue=off;
                            reference_turn=reference_turn+change_one_bottle;
                            n=1;
                        }                     
                        
                        if (fabs(position_turn-reference_turn)<2)                      // als error en kleiner dan twee graden
                            {
                            debug_led_blue=off;
                            debug_led_red=on;
                            wait(0.5);
                            if (fabs(position_turn-reference_turn)<2)                  // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
                                {
                                goto Nieuwe_actie;                                     // kunt weer iets nieuws doen indien vorige actie is uitgevoerd  
                                }
                            }
                    }
            }
        
        debug_led_red=off;                                                             // not inside an action loop
        debug_led_blue=off;
        debug_led_green=on;                                                            // not executing an action
        
        
        // DRAAI RECHTS                                                                // (blue->blue uit (heel kort/zichtbaar?)-> red<->blue<-> red -> klaar)
        if (EMG_R > Threshold_Bicep_Right_2 && EMG_L < Threshold_Bicep_Right_1)
            {
                debug_led_green=off;                                                   // Executing action
                pc.printf("rechts \n\r");                  
                n=0;
                while(EMG_R > Threshold_Bicep_Right_1 && EMG_L < Threshold_Bicep_Left_1)
                    {
                        debug_led_blue=on;
                        if (n==0)
                        {
                            debug_led_blue=off;
                            reference_turn=reference_turn-change_one_bottle;
                            n=1;
                        }                                                             //(45 graden naar rechts voor volgende fles) //wordt maar 1 keer uitgevoerd
                        
                        if (abs(position_turn-reference_turn)<2)                      // als error en kleiner dan twee graden
                            {
                            debug_led_blue=off;
                            debug_led_red=on;
                            wait(0.5);
                            if (abs(position_turn-reference_turn)<2)                  // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
                                {
                                goto Nieuwe_actie;                                    // kunt weer iets nieuws doen indien vorige actie is uitgevoerd  
                                }
                            }
                    }
            }
                
        debug_led_red=off;                                                            // not inside an action loop
        debug_led_blue=off;
        debug_led_green=on;                                                           // not executing an action
                    
                
                if(send_flag)
                    {
                        send_flag = false;
                        scope.set(0,EMG_R);                                           // HIDSCOPE channel 0 : EMG_R
                        scope.set(1,EMG_L);                                           // HIDSCOPE channel 1 : EMG_L
                        scope.set(2,reference_turn);                                  // HIDSCOPE channel 2 : Reference_Turn
                        scope.send();                                                 // Send channel info to HIDSCOPE
                    }
            } // end while loop
}


// Keep in range function
void keep_in_range(double * in, double min, double max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}

// Looptimerflag function
void send(void)
{
    send_flag = true;
}

// Deactivate PID Controller strike
void deactivate_PID_Controller_strike(void)
{
    double P_gain_strike=0;
    double I_gain_strike=0;
    double D_gain_strike=0;
}

// Activate PID Controller strike
void activate_PID_Controller_strike(void)
{
    double P_gain_strike=10;
    double I_gain_strike=0.1;
    double D_gain_strike=50;
}