Bouke Scheltinga
Werkcollege opgave 23 september BMT K9
Dependencies: Encoder HIDScope MODSERIAL mbed QEI biquadFilter
main.cpp
- Committer:
- ThomasBNL
- Date:
- 2015-10-15
- Revision:
- 33:61696d5fa735
- Parent:
- 31:4ddf83ff4295
- Child:
- 34:7c51b08f1aa2
File content as of revision 33:61696d5fa735:
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
/////// ////// ////// ////// ////// /////// ////// /////// ////// /////////////////////////////////////////////|
/////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
/////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
/////// ///////////////// ////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
/////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
/////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
/////// ///////////////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
/////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
/////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
/////// ////// ////// ////// ////// ////// ////// ////// ////// /////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
/////// /////////////// ///////////// /////// ////// /////////////////////////////////////////////////////////////////////////|
/////// ///////////// //////////// ///// ////// ////////////////////////////////////////////////////////////////////////|
/////// ////// /////////// /////////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// ////////// // ////////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ///////// //// ///////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// //////// ////// //////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// /////// /////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// ////// ////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// //////////// ///// ///// ////// ////////////////////////////////////////////////////////////////////////|
/////// ////// ////////////// //// ////// ////// /////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
// Groep 12: The Chenne Band //
// ___________________________
// // \\
// || [Table of content] ||
// \\___________________________//
//
// Introduction
// Libraries.................................................
// Flow and debugging tools .................................
// LEDS.................................................
// Buttons..............................................
// Potmeter.............................................
// Flow.................................................
// HIDscope.............................................
// EMG.......................................................
// EMG variables........................................
// EMG filter...........................................
// motors.....................................................
// motor turn...........................................
// motor strike.........................................
// d-action filter......................................
// system constants...........................................
// functions used.............................................
// motor function.......................................
// EMG functions........................................
// action controller....................................
// Main
// Start of code..............................................
// calibrate..................................................
// starting position motor.............................. (RED LED)
// EMG calibration...................................... (BLUE LED)
// Attach Ticker..............................................
// Start infinite loop........................................
// Debug buttons........................................
// Wait for go signal...................................
// Limit position value and convert to degrees..........
// Get current EMG values...............................
// Action controller....................................
// Strike.........................................
// Turn left......................................
// Turn right.....................................
// PID controller.......................................
// PID error -> pwm motor...............................
// pwm -> plant.........................................
// HIDscope.............................................
// Deactivate if reached position.......................
// Functions described
// Motor Functions............................................
// EMG functions..............................................
// Action controller functions................................
//
//
//
//
//
//============================================================================================================================
// ___________________________
// // \\
// || [Libraries] ||
// \\___________________________//
//
#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "biquadFilter.h"
#include "encoder.h"
//============================================================================================================================
// ___________________________
// // \\
// || [FLOW AND DEBUGGING TOOLS] ||
// \\___________________________//
// _________________________
// : [LEDS] :
// :__________________________:
//
DigitalOut debug_led_red(LED_RED); // Debug LED
DigitalOut debug_led_green(LED_GREEN); // Debug LED
DigitalOut debug_led_blue(LED_BLUE); // Debug LED
// __________________________
// : [Buttons] :
// :__________________________:
//
DigitalIn buttonL1(PTC6); // Button 1 (low on k64f) for testing at the lower board
DigitalIn buttonL2(PTA4); // Button 2 (low on k64f) for testing at the lower board
DigitalIn buttonH1(D2); // Button 3 (high on k64f) for testing at the top board
DigitalIn buttonH2(D6); // Button 4 (high on k64f) for testing at the top board
// __________________________
// : [Potmeter] :
// :__________________________:
// DEBUGGING / TESTING
//moving_average_right = (potmeter1.read())*100; // EMG Right bicep (tussen nul en 100%)
//moving_average_left = (potmeter2.read())*100; // EMG Left bicep (tussen nul en 100%)
// __________________________
// : [FLOW] :
// :__________________________:
//
Ticker looptimer; // Ticker called looptimer to set a looptimerflag
// __________________________
// : [HIDSCOPE] :
// :__________________________:
//
HIDScope scope(3); // HIDSCOPE declared
MODSERIAL pc(USBTX,USBRX);
//============================================================================================================================
// ___________________________
// // \\
// || [EMG] ||
// \\___________________________//
//
AnalogIn input1(A0); // EMG: Two AnalogIn EMG inputs
AnalogIn input2(A1); // EMG: Two AnalogIn EMG inputs
// __________________________
// : [EMG variables] :
// :__________________________:
//
volatile bool control_go = false; // EMG:
volatile bool sample = false; // EMG:
//
double Sample_EMG_L_1, Sample_EMG_L_2, Sample_EMG_L_3, Sample_EMG_L_4, Sample_EMG_L_5, Sample_EMG_L_6, Sample_EMG_L_7, Sample_EMG_L_8, Sample_EMG_L_9, Sample_EMG_L_10, moving_average_left; // EMG:
double Sample_EMG_R_1, Sample_EMG_R_2, Sample_EMG_R_3, Sample_EMG_R_4, Sample_EMG_R_5, Sample_EMG_R_6, Sample_EMG_R_7, Sample_EMG_R_8, Sample_EMG_R_9, Sample_EMG_R_10, moving_average_right; // EMG:
double minimum_L; // EMG:
double maximum_L; // EMG:
double EMG_L_min; // EMG:
double EMG_L_max; // EMG:
double minimum_R; // EMG:
double maximum_R; // EMG:
double EMG_R_min; // EMG:
double EMG_R_max; // EMG:
double n=0; // EMG:
double c=0; // EMG:
// __________________________
// : [EMG Filter] :
// :__________________________:
// Biquad filters
const double low_b0 = 0.05892937945281792;
const double low_b1 = 0.11785875890563584;
const double low_b2 = 0.05892937945281792;
const double low_a1 = -1.205716572226748;
const double low_a2 = 0.44143409003801976; // VIA online biquad calculator Lowpas 520-48-0.7071-6
const double high_b0 = 0.6389437261127494;
const double high_b1 = -1.2778874522254988;
const double high_b2 = 0.6389437261127494;
const double high_a1 = -1.1429772843080923;
const double high_a2 = 0.41279762014290533; // VIA online biquad calculator Highpas 520-52-0.7071-6
biquadFilter highpassfilter_1(high_a1, high_a2, high_b0, high_b1, high_b2);
biquadFilter highpassfilter_2(high_a1, high_a2, high_b0, high_b1, high_b2);
biquadFilter lowpassfilter_1(low_a1, low_a2, low_b0, low_b1, low_b2);
biquadFilter lowpassfilter_2(low_a1, low_a2, low_b0, low_b1, low_b2);
double EMG_left_Bicep; double EMG_Right_Bicep; // input variables
double EMG_Left_Bicep_filtered; double EMG_Right_Bicep_filtered; // output variables
//============================================================================================================================
// ___________________________
// // \\
// || [MOTORS] ||
// \\___________________________//
//
// __________________________
// : [Motor TURN] :
// :__________________________:
//
QEI motor_turn(D12,D13,NC,32); // TURN: Encoder for motor
PwmOut pwm_motor_turn(D5); // TURN: Pwm for motor
DigitalOut motordirection_turn(D4); // TURN: Direction of the motor
// PID motor constants
double integrate_error_turn=0; // TURN: integration error turn motor
double previous_error_turn=0; // TURN: previous error turn motor
double position_turn; // TURN: Set variable to store current position of the motor
double error_turn; // TURN: Set variable to store the current error of the motor
double pwm_motor_turn_P; // TURN: variable that stores the P action part of the error
double pwm_motor_turn_I; // TURN: variable that stores the I action part of the error
double pwm_motor_turn_D; // TURN: variable that stores the D action part of the error
double pwm_to_motor_turn; // TURN: variable that is constructed by summing the values of the P, I and D action
double P_gain_turn; // TURN: this gain gets multiplied with the error inside the P action of the PID controller
double I_gain_turn; // TURN: this gain gets multiplied with the error inside the I action of the PID controller
double D_gain_turn; // TURN: this gain gets multiplied with the error inside the D action of the PID controller
double reference_turn; // TURN: reference position of the motor (position the motor 'likes' to get to)
// __________________________
// : [Motor STRIKE] :
// :__________________________:
//
//QEI motor_strike(XX,XX,NC,32); // STRIKE: Encoder for motor Turn
//PwmOut pwm_motor_strike(XX); // STRIKE: Pwm for motor Turn
//DigitalOut motordirection_strike(XX); // STRIKE: Direction of the motor Turn
// PID motor constants
//double integrate_error_strike=0; // STRIKE: integration error turn motor
//double previous_error_strike=0; // STRIKE: previous error turn motor
//double position_strike; // STRIKE: Set variable to store current position of the motor
//double error_strike; // STRIKE: Set variable to store the current error of the motor
//double pwm_motor_strike_P; // STRIKE: variable that stores the P action part of the error
//double pwm_motor_strike_I; // STRIKE: variable that stores the I action part of the error
//double pwm_motor_strike_D; // STRIKE: variable that stores the D action part of the error
//double pwm_to_motor_strike; // STRIKE: variable that is constructed by summing the values of the P, I and D action
//double reference_strike;
//double P_gain_turn; // STRIKE: this gain gets multiplied with the error inside the P action of the PID controller
//double I_gain_turn; // STRIKE: this gain gets multiplied with the error inside the I action of the PID controller
//double D_gain_turn; // STRIKE: this gain gets multiplied with the error inside the D action of the PID controller
//double reference_strike; // STRIKE: reference position of the motor (position the motor 'likes' to get to)
// __________________________
// : [D-action filter] :
// :__________________________:
// Applicable for both motors
const double mT_f_a1=-1.965293372622690e+00; // Motor Turn derivative filter constants
const double mT_f_a2=9.658854605688177e-01;
const double mT_f_b0=1.480219865318266e-04;
const double mT_f_b1=2.960439730636533e-04;
const double mT_f_b2=1.480219865318266e-04;
biquadFilter Lowpassfilter_derivative(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2); // BiquadFilter used for the filtering of the Derivative action of the PID-action
//============================================================================================================================
// ___________________________
// // \\
// || [SYSTEM CONSTANTS] ||
// \\___________________________//
//
volatile bool looptimerflag;
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 int Fs = 512; // sampling frequency (512 Hz)
const double sample_time = 0.001953125; // duration of one sample
double conversion_counts_to_degrees=0.085877862594198; // Calculation conversion counts to degrees
// gear ratio motor = 131
// ticks per magnet rotation = 32 (X2 Encoder)
// One revolution = 360 degrees
// degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
const double change_one_bottle=45;
//============================================================================================================================
// ___________________________
// // \\
// || [FUNCTIONS USED] ||
// \\___________________________//
// __________________________
// : [Motor Functions] :
// :__________________________:
// Applicable for both motors
void keep_in_range (double * in, double min, double max); // Put in a value and makes sure that the value stays between assigned boundary's
void setlooptimerflag (void); // Sets looptimerflag volatile bool to true
void deactivate_PID_Controller (double& P_gain, double& I_gain, double& D_gain); // STRIKE/TURN: Deactivate PID Controller
void activate_PID_Controller_strike (double& P_gain, double& I_gain, double& D_gain); // STRIKE: Activate PID Controller
void activate_PID_Controller_turn (double& P_gain, double& I_gain, double& D_gain); // TURN: Activate PID Controller
void Change_Turn_Position_Left (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the left
void Change_Turn_Position_Right (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the right
// __________________________
// : [EMG Functions] :
// :__________________________:
// Applicable for both biceps
void calibration(); // EMG: Calibrate the EMG signal (calculate min and max signal and determine threshold values)
void countdown_from_5(); // PUTTY: 5 second countdown inside
void Filter(); // EMG: Filter the incoming EMG signals
void sample_filter(); // EMG: Calculate moving average (10 samples, one sample per 25 samples) using sample_filter => moving average over +-0.5 seconds
void take_sample(); // EMG: Take a sample once every 25 samples that's used to calculate the moving average
void ControlGo(); // EMG: function that gets a ticker attached and sets a certain loop to true every sample
// __________________________
// : [Action Controller] :
// :__________________________:
//
//============================================================================================================================
///////////////////////////////
// //
///////////////////////////////////////////// [MAIN FUNCTION] /////////////////////////////////////////////////////
// //
///////////////////////////////
//============================================================================================================================
int main() {
debug_led_red=off;
debug_led_blue=off;
debug_led_green=off;
// ___________________________
// // \\
// || [START OF CODE] ||
// \\___________________________//
// START OF CODE
pc.printf("Start of code \n\r");
pc.baud(115200); // Set the baudrate
// ___________________________
// // \\
// || [CALIBRATE] ||
// \\___________________________//
// Calibrate starting postion (RED LED)
// ___________________________
// : [Starting position motor] :
// :___________________________:
//
pc.printf("Button calibration \n\r");
while(1)
{
debug_led_red=on; // TURN: LOW buttons
if (buttonL2.read() < 0.5){ // TURN: turn the motor clockwise with pwm of 0.2
motordirection_turn = cw;
pwm_motor_turn = 0.2f;}
if (buttonL1.read() < 0.5){ // TURN: turn the motor counterclockwise with pwm of 0.2
motordirection_turn = ccw;
pwm_motor_turn = 0.2f;}
// STRIKE: HIGH buttons
// if (buttonH2.read() < 0.5){ // STRIKE: turn the motor clockwise with pwm of 0.2
// motordirection_strike = cw;
// pwm_motor_turn = 0.2f;}
//
// if (buttonH1.read() < 0.5){ // STRIKE: turn the motor clockwise with pwm of 0.2
// motordirection_strike = ccw;
// pwm_motor_turn = 0.2f;}
//
if ((buttonL2.read() < 0.5) && (buttonL1.read() < 0.5)) // Save current TURN and STRIKE positions as starting position
{
motor_turn.reset(); // TURN: Starting Position
reference_turn=0; // TURN: Set reference position to zero
// motor_strike.reset(); // STRIKE: Starting Position
// double reference_strike=0; // STRIKE: Set reference position to zero
goto calibration_starting_position_complete; // Calibration complete
}
}
calibration_starting_position_complete:
debug_led_red=off; // Calibration end => RED LED off
// ___________________________
// : [EMG calibration] :
// :___________________________:
//
calibration(); // EMG: Calibration
// ___________________________
// // \\
// || [ATTACH TICKER] ||
// \\___________________________//
// Attach Ticker to looptimerflag
// EMG_ticker.attach(&ControlGo, (float)1/Fs); // EMG: Ticker
looptimer.attach(setlooptimerflag,(float)1/Fs); // calls the looptimer flag every sample
pc.printf("Start infinite loop \n\r");
wait (5); // Wait 5 seconds before starting system
activate_PID_Controller_strike(P_gain_turn, I_gain_turn, D_gain_turn);
// ___________________________
// // \\
// || [START INFINTTE LOOP] ||
// \\___________________________//
//
while(1)
{ // Start while loop
// ___________________________
// // \\
// || [DEBUG BUTTONS] ||
// \\___________________________//
// interrupt button Disbalances the current motor position
//if button L2 pressed => disbalance motor
if (buttonL2.read() < 0.5){
motordirection_turn = cw;
pwm_motor_turn = 0.5f;
pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
// if button L1 pressed => shut down motor for 1000 seconds
if (buttonL1.read() < 0.5){
motordirection_turn = cw;
pwm_motor_turn = 0;
wait(1000);
pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
else
{
// ___________________________
// // \\
// || [Wait for go signal] ||
// \\___________________________//
// // Wait until looptimer flag is true then execute PID controller loop.
while(looptimerflag != true);
looptimerflag = false;
// __________________________________________________
// // \\
// || [Limit position value and convert to degrees] ||
// \\__________________________________________________//
// // Keep motor position between -4200 and 4200 counts
if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
{
motor_turn.reset();
pc.printf("RESET \n\r");
}
// Convert position to degrees \\
position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
// ___________________________
// // \\
// || [GET CURRENT EMG VALUES] ||
// \\___________________________//
//
debug_led_green = on; //
sample_filter();
// scope.set(0,EMG_left_Bicep);
// scope.set(1,EMG_Left_Bicep_filtered);
// scope.set(2,moving_average_left);
// scope.send(); //
// ___________________________
// // \\
// || [ACTION CONTROLLER] ||
// \\___________________________//
//
// DO STUFF / DECIDE WHETHER TO DO SOMETHING OR NOT ==> TICKER loop moet dan wel uit (TICKER VOOR SAMPLE FILTER WEL AAN)
// Nieuwe_actie:
// TICKER AAN // 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);
// moving_average_right = (potmeter1.read())*100; //EMG Right bicep (tussen nul en 100%)
// moving_average_left = (potmeter2.read())*100; // EMG Left bicep (tussen nul en 100%)
// pc.printf("moving_average_left = %f moving_average_right = %f \n\r", moving_average_left, moving_average_right);
//
//
// // STRIKE // (No LED -> 0.55s -> red on (turns on every time a new pwm is given) -> finish)
// if (moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1)
// {
// TICKER OFF
// debug_led_green=off;
// n=0;
// pc.printf("slag \n\r");
// wait(0.5);
//
// while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > 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=((moving_average_left-EMG_L_min)+(moving_average_right-EMG_R_min))/((EMG_L_max-EMG_L_min)+(EMG_R_max-EMG_R_min))*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
// TICKER AAN
// 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
//
// // TURN LEFT // // (blue->blue off (very short/visible?)-> red<->blue<-> red -> klaar)
// if (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1)
// {
// debug_led_green=off; // Executing action
// TICKER UIT
// n=0;
// pc.printf("links \n\r");
// while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right < 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
// TICKER AAN
//
// // TURN RIGHT // (blue->blue uit (heel kort/zichtbaar?)-> red<->blue<-> red -> klaar)
// if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Right_1)
// {
// TICKER UIT
// debug_led_green=off; // Executing action
// pc.printf("rechts \n\r");
// n=0;
// while(moving_average_right > Threshold_Bicep_Right_1 && moving_average_left < 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
// pc.printf("Ref: %f Err: %f \n\r", reference_turn, position_turn);
//
// 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
//
// TICKER WEER AAN
// ___________________________
// // \\
// || [PID CONTROLLER] ||
// \\___________________________//
// // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\
error_turn=(reference_turn - position_turn); // TURN: Current error (input controller)
integrate_error_turn=integrate_error_turn + sample_time*error_turn; // TURN: Integral error output
// overwrite previous integrate error by adding the current error
// multiplied by the sample time.
//
double error_derivative_turn=(error_turn - previous_error_turn)/sample_time; // TURN: Derivative error output
error_derivative_turn=Lowpassfilter_derivative.step(error_derivative_turn); // TURN: Filter
previous_error_turn=error_turn; // current error is saved to memory constant to be used in
// the next loop for calculating the derivative error
pwm_motor_turn_P = error_turn*P_gain_turn; // Output P controller to pwm
pwm_motor_turn_I = integrate_error_turn*I_gain_turn; // Output I controller to pwm
pwm_motor_turn_D = error_derivative_turn*D_gain_turn; // Output D controller to pwm
pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D;
// ___________________________
// // \\
// || [PID error -> pwm motor] ||
// \\___________________________//
// // Keep Pwm between -1 and 1 -> and decide the direction of the motor to compensate the error
keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to motor controller but keep it in range!
pc.printf("pwm %f \n\r", pwm_to_motor_turn);
// Check error and decide direction to turn
if(pwm_to_motor_turn > 0)
{
motordirection_turn=ccw;
pc.printf("if loop pwm > 0 \n\r");
}
else
{
motordirection_turn=cw;
pc.printf("else loop pwm < 0 \n\r");
}
// ___________________________
// // \\
// || [pwm -> Plant] ||
// \\___________________________//
// // Put pwm to the motor \\
pwm_motor_turn=(abs(pwm_to_motor_turn));
// ___________________________
// // \\
// || [HIDSCOPE] ||
// \\___________________________//
// // Check signals inside HIDSCOPE \\
scope.set(0,error_turn); // HIDSCOPE channel 0 : Current Error
scope.set(1,position_turn); // HIDSCOPE channel 1 : Position_turn
scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn
scope.send(); // Send channel info to HIDSCOPE
// __________________________________
// // \\
// || [Deactivate if reached position] ||
// \\__________________________________//
// // Check whether the motor has reached reference position and can be shut down
if(fabs(error_turn)<2) // Shut down if error is smaller than two degrees
{deactivate_PID_Controller(P_gain_turn,I_gain_turn,D_gain_turn);}
else
{activate_PID_Controller_turn(P_gain_turn,I_gain_turn,D_gain_turn);}
}
}
}
//============================================================================================================================
///////////////////////////////
// //
///////////////////////////////////////////// [FUNCTIONS DESCRIBED] /////////////////////////////////////////////////////
// //
///////////////////////////////
//============================================================================================================================
// ___________________________
// // \\
// || [MOTOR FUCNTIONS] ||
// \\___________________________//
//
// 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;
}
// Change reference
void Change_Turn_Position_Left (double& reference, double change_one_bottle)
{
if(reference==90) // added reference if at boundary bottle and try to go to the side where no bottles are; than immediatly turn 5 bottles to the left
{
reference=-90;
}
else
{
reference = reference + change_one_bottle;
keep_in_range(&reference, -90, 90); // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
}
}
void Change_Turn_Position_Right (double& reference, double change_one_bottle)
{
if(reference==-90)
{
reference=90;
}
else
{
reference = reference - change_one_bottle;
keep_in_range(&reference, -90, 90);
}
}
// Deactivate or Activate PID_Controller
void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain)
{
P_gain=0;
I_gain=0;
D_gain=0;
}
void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
{
P_gain=0.01; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
I_gain=0.5;
D_gain=5;
}
void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
{
P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
I_gain=0;
D_gain=0;
}
//============================================================================================================================
// ___________________________
// // \\
// || [EMG FUCNTIONS] ||
// \\___________________________//
//
// [CALIBRATION] // (blue LED)
void calibration()
{
// [MINIMUM VALUE BICEPS CALIBRATION] //
debug_led_blue=on;
pc.printf("Start minimum calibration in 5 seconds \n\r");
pc.printf("Keep your biceps as relaxed as possible \n\r");
countdown_from_5();
c=0;
while(c<2560) // 512Hz -> 2560 is equal to five seconds
{
Filter(); // Filter EMG signal
minimum_L=EMG_Left_Bicep_filtered+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
minimum_R=EMG_Right_Bicep_filtered+minimum_R;
// scope.set(0,EMG_left_Bicep);
// scope.set(1,EMG_Left_Bicep_filtered);
// scope.set(2,minimum_L);
// scope.send();
c++; // Every sample c is increased by one until the statement c<2560 is false
wait(0.001953125); // wait one sample
}
pc.printf("Finished minimum calibration \n\r");
EMG_L_min=minimum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
EMG_R_min=minimum_R/2560;
pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);
wait (3); //cooldown
// [MAXIMUM VALUE BICEPS CALIBRATION] //
pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
countdown_from_5();
c=0;
while(c<2560) // 512Hz -> 2560 is equal to five seconds
{
Filter(); // Filter EMG signal
maximum_L=EMG_Left_Bicep_filtered+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
maximum_R=EMG_Right_Bicep_filtered+maximum_R;
// scope.set(0,EMG_left_Bicep);
// scope.set(1,EMG_Left_Bicep_filtered);
// scope.set(2,maximum_R);
// scope.send();
c++; // Every sample c is increased by one until the statement c<2560 is false
wait(0.002);
}
pc.printf("Finished minimum calibration \n\r");
EMG_L_max=maximum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
EMG_R_max=maximum_R/2560;
pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f \n\r", EMG_L_max, EMG_R_max);
wait (3); //cooldown
debug_led_blue=off;
// [MAXIMUM VALUE BICEPS CALIBRATION] //
// Calculate threshold percentages //
const float Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*0.2)+EMG_L_min;; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
const float Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6)+EMG_L_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
const float Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*0.2)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
const float Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
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);
}
// [COUNTDOWN FUNCTION] //
void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
{
wait(1); pc.printf("5 \n\r"); wait(1); pc.printf("4 \n\r"); wait(1); pc.printf("3 \n\r"); wait(1); pc.printf("2 Ready \n\r");
wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r");
}
// [FLOW CONTROL FUNCTIONS] //
void ControlGo() //Control flag
{
control_go = true;
}
void take_sample() // Take a sample every 25th sample
{
if(n==25)
{
sample = true;
n=0;
debug_led_green = off;
}
}
// [FILTER FUNCTIONS] //
// [EMG] //
void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
{
EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered);
EMG_Left_Bicep_filtered = EMG_Left_Bicep_filtered; EMG_Right_Bicep_filtered = EMG_Right_Bicep_filtered;
}
// [FILTER FUNCTIONS] //
// [Include Moving Average] //
void sample_filter()
{
Filter();
take_sample();
if(sample)
{
sample=false;
Sample_EMG_L_1 = EMG_Left_Bicep_filtered; Sample_EMG_R_1 = EMG_Right_Bicep_filtered;
Sample_EMG_L_10= Sample_EMG_L_9; Sample_EMG_R_10= Sample_EMG_R_9;
Sample_EMG_L_9 = Sample_EMG_L_8; Sample_EMG_R_9 = Sample_EMG_R_8;
Sample_EMG_L_8 = Sample_EMG_L_7; Sample_EMG_R_8 = Sample_EMG_R_7;
Sample_EMG_L_7 = Sample_EMG_L_6; Sample_EMG_R_7 = Sample_EMG_R_6;
Sample_EMG_L_6 = Sample_EMG_L_5; Sample_EMG_R_6 = Sample_EMG_R_5;
Sample_EMG_L_5 = Sample_EMG_L_4; Sample_EMG_R_5 = Sample_EMG_R_4;
Sample_EMG_L_4 = Sample_EMG_L_3; Sample_EMG_R_4 = Sample_EMG_R_3;
Sample_EMG_L_3 = Sample_EMG_L_2; Sample_EMG_R_3 = Sample_EMG_R_2;
Sample_EMG_L_2 = Sample_EMG_L_1; Sample_EMG_R_2 = Sample_EMG_R_1;
}
moving_average_left=Sample_EMG_L_1*0.1+Sample_EMG_L_2*0.1+Sample_EMG_L_3*0.1+Sample_EMG_L_4*0.1+Sample_EMG_L_5*0.1+Sample_EMG_L_6*0.1+Sample_EMG_L_7*0.1+Sample_EMG_L_8*0.1+Sample_EMG_L_9*0.1+Sample_EMG_L_10*0.1;
moving_average_right=Sample_EMG_R_1*0.1+Sample_EMG_R_2*0.1+Sample_EMG_R_3*0.1+Sample_EMG_R_4*0.1+Sample_EMG_R_5*0.1+Sample_EMG_R_6*0.1+Sample_EMG_R_7*0.1+Sample_EMG_R_8*0.1+Sample_EMG_R_9*0.1+Sample_EMG_R_10*0.1;
n++;
}
//============================================================================================================================
// ___________________________
// // \\
// || [Action Controller] ||
// \\___________________________//
//
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 23 Sep 2015 |
| Imports: | 4 |
| Forks: | 0 |
| Commits: | 53 |
| Dependents: | 0 |
| Dependencies: | 6 |
| Followers: | 4 |
