File content as of revision 27:b7caf668a682:

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

MODSERIAL   pc(USBTX,USBRX);
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

// (DEBUGGING AND TESTING BUTTONS) (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

//volatile bool looptimerflag;
const double  cw=0;                     // zero is clockwise (front view)
const double  ccw=1;                    // one is counterclockwise (front view)

const double sample_time=0.01;          // tijd voor een sample (100Hz)
  
// Functions used (described after main)
// void setlooptimerflag(void);


// MAIN function
int main() {
    debug_led_red=0;
    debug_led_blue=0;
    debug_led_green=0;
    AnalogIn potmeter1(A0); // POTMETER -> DOEN ALSOF EMG SIGNAAL
    AnalogIn potmeter2(A1); // POTMETER -> DOEN ALSOF EMG SIGNAAL
    
    QEI motor_turn(D12,D13,NC,32);          // Encoder for motor Turn
    PwmOut pwm_motor_turn(D5);              // Pwm for motor Turn
    DigitalOut motordirection_turn(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 error;
    double pwm_to_motor_turn;
    double pwm_motor_turn_P;
    double pwm_motor_turn_I;
    double pwm_motor_turn_D;
    
    //START OF CODE 
    pc.printf("Start of code \n\r");
    
    pc.baud(115200);                          // Set the baudrate
    
    // Tickers 
    Ticker looptimer;                                          // Ticker called looptimer to set a looptimerflag
    looptimer.attach(setlooptimerflag,sample_time);            // calls the looptimer flag every 0.01s
    
    //Sample_Ticker.attach(&get_sample, sample_time); // HIDSCOPE sample Ticker
    
    pc.printf("Start infinite loop \n\r");
    wait (3);                                                  // Wait before starting system
    
    //INFINITE LOOP
    while(1) 
        {                                                                                                   // Start while loop}      
        // Wait until looptimer flag is true then execute PID controller.  
        while(looptimerflag != true);
        looptimerflag = false;
        
        // INPUT EMG SIGNAAL
        EMG_R = (potmeter1.read()-0.5)*2000;  //EMG Right bicep 
        EMG_L = (potmeter2.read()-0.5)*2000; // EMG Left bicep
        
        // calibratie
        double max_EMG = 40; // Calibreren
        double min_EMG = 0;
        double Hit=60; // position when bottle is hit
        
        // REGELAAR (wat gaat de controller doen om bestuurd te worden)
        const double Threshold_Bicep_Left_1=10;  // percentages van min en max EMG 
        const double Threshold_Bicep_Left_2=20;
        const double Threshold_Bicep_Left_3=30;
        const double Threshold_Bicep_Right_1=10;
        const double Threshold_Bicep_Right_2=20;
        
        Nieuwe_actie:  // Komt hier weer terecht na
        
        
        // SLAG
        if (EMG_L > Threshold_Bicep_Left_1 || EMG_R > Threshold_Bicep_Right_1)
            {
            wait(0.5); //(wacht 0.5s om te kijken hoe hard geslagen moet worden)
            while(EMG_L > Threshold_Bicep_Left_1 || EMG_R > Threshold_Bicep_Right_1) // Nog steeds over de threshold?
                {
                    // deactivate_PID_Controller_strike // function that sets P, I and D gain values to zero
                    wait(0.1); // wacht 10 samples
                pwm_to_motor_strike=((EMG_L+EMG_R)/(max_EMG+min_EMG))*0.7 + 0.3 // min speed 0.3 en max 1
                motor_strike_direction=cw; // towards bottle
                
                if((Position-Hit)<2) // geraakt als position-hit <2 graden
                        (
                        // activate_PID_Controller_strike // function sets back P, I and D gain values
                        goto Nieuwe_actie;  // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
                        }
                }
                
        // activate_PID_Controller_strike // function sets back P, I and D gain values 
        // want het kan zijn dat bovenste script half wordt uitgevoerd en dan moet het slag mechanisme wel weer terug
        
        // DRAAI LINKS
        else if (EMG_L > Threshold_Bicep_Left_2 || EMG_R < Threshold_Bicep_Right_1)
            {  
                
                n=0;
                while(EMG_L > Threshold_Bicep_Left_1 || EMG_R < Threshold_Bicep_Right_1)
                    {
                        if (n==0)
                        {
                            Reference=Reference+45;
                            n=1;
                        }                      //(45 graden naar links voor volgende fles) //wordt maar 1 keer uitgevoerd
                        
                        if (abs(error)<2) // als error en kleiner dan twee graden
                            {
                            wait(0.5);
                            if (abs(error)<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  
                                }
                            }
                    }
            }
        
        // DRAAI RECHTS
        else if (EMG_R > Threshold_Bicep_Left_2 || EMG_L < Threshold_Bicep_Right_1)
            {                  
                n=0;
                while(EMG_R > Threshold_Bicep_Left_1 || EMG_L < Threshold_Bicep_Right_1)
                    {
                        if (n==0)
                        {
                            Reference=Reference-45;
                            n=1;
                        }                 //(45 graden naar rechts voor volgende fles) //wordt maar 1 keer uitgevoerd
                        
                        if (abs(error)<2) // als error en kleiner dan twee graden
                            {
                            wait(0.5);
                            if (abs(error)<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  
                                }
                            }
                    }
            }

        // Send HIDSCOPE data
//            scope.set(0,reference_turn); // HIDSCOPE channel 0 : Current Reference
//            scope.set(0,position_turn); // HIDSCOPE channel 0 : Position_turn
//            scope.set(1,pwm_to_motor_turn); // HIDSCOPE channel 1 : Pwm_to_motor_turn
//            scope.send();   // Send channel info to HIDSCOPE
//        } 
    }
}
}

// 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 setlooptimerflag(void)
{
    looptimerflag = true;
}

//// Get sample
//void get_sample(void) // HIDSCOPE sample fuction
//{
//    sample = true;
//}