Thomas Burgers
The Chenne Robot
Dependencies: Encoder HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp
- Committer:
- ThomasBNL
- Date:
- 2015-11-03
- Revision:
- 110:58c6a32659d3
- Parent:
- 108:873e56f92691
File content as of revision 110:58c6a32659d3:
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
/////// ////// ////// ////// ////// /////// ////// /////// ////// /////////////////////////////////////////////|
/////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
/////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
/////// ///////////////// ////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
/////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
/////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
/////// ///////////////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
/////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
/////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
/////// ////// ////// ////// ////// ////// ////// ////// ////// /////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
/////// /////////////// ///////////// /////// ////// /////////////////////////////////////////////////////////////////////////|
/////// ///////////// //////////// ///// ////// ////////////////////////////////////////////////////////////////////////|
/////// ////// /////////// /////////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// ////////// // ////////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ///////// //// ///////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// //////// ////// //////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// /////// /////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// ////// ////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// //////////// ///// ///// ////// ////////////////////////////////////////////////////////////////////////|
/////// ////// ////////////// //// ////// ////// /////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
// Groep 12: The Chenne Band ///
//============================================================================================================================
// ___________________________
// // \\
// || [Libraries] ||
// \\___________________________//
//
#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "biquadFilter.h"
#include "encoder.h"
//============================================================================================================================
// ___________________________
// // \\
// || [Tuning Device] ||
// \\___________________________//
//
// Calculated PID values (MATLAB)
const double tuning1=0.0041211; // P_gain_strike value
const double tuning2=0.039481; // I_gain_strike value
const double tuning3=0; // D_gain_strike value
const double tuning4=0.041211; // P_gain_turn value
const double tuning5=0.39481; // I_gain_turn value
const double tuning6=0; // D_gain_turn value
// FILTER: D-action
const double a0 = 0.000021080713160785432, a1 = 0.000042161426321570865, a2 = 0.000021080713160785432, b1 = -1.990754082898736, b2 = 0.9908384057513788; //(0.75Hz Lowpass filter)
biquadFilter Lowpassfilter_derivative(b1,b2,a0,a1,a2); // BiquadFilter used for the filtering of the Derivative action of the PID-action
// MOVEMENT ROBOT TUNING
const double change_one_bottle=25; // Number of degrees between two bottles
const double Hit=25; // Number of ccw degrees in comparison to starting position when bottle is hit
const double tuning24=0.10; // if <smaller than tuning18 than execute tuning 24 to prevent being stuck inside this loop (minimum pwm)
const double tuning18=0.1, tuning25=0.15; // First value: How much muscle percentage force is required (between 0-1)
const double tuning19=0.2, tuning26=0.21; // Second value: If after measurement period this value is the last (than what speed (pwm) does the motor get)
const double tuning20=0.3, tuning27=0.28;
const double tuning21=0.5, tuning28=0.38;
const double tuning22=0.7, tuning29=0.55;
const double tuning23=0.9, tuning30=0.65;
// TURN LEFT
const double tuning13=1.75; // error turn average smaller than (ENTRY) degrees than continue (BOTH FOR TURN LEFT AND TURN ACTION!
const double tuning14=150; // how many samples at least before strike error is small enough (BOTH FOR TURN LEFT AND TURN ACTION!
//============================================================================================================================
// ___________________________
// // \\
// || [FLOW AND DEBUGGING TOOLS] ||
// \\___________________________//
//HIDScope scope(1); // DEBUG : HIDSCOPE has the ability to display signals over time and can be used to monitor signals
MODSERIAL pc(USBTX,USBRX); // MODSERIAL : makes it possible to send messages to the computer (eg. inside Putty)
DigitalOut debug_led_red(LED_RED) , debug_led_green(LED_GREEN) , debug_led_blue(LED_BLUE); // DEBUG : Red, Blue and Green LED
DigitalIn buttonL1(PTC6) , buttonL2(PTA4) , buttonH1(D2) , buttonH2(D1); // DEBUG/CALIBRATION: 4 buttons for calibration and debugging
Ticker looptimer; // FLOW : Ticker called looptimer to set a looptimerflag that puts the volatile bool control_go to true every sample
DigitalOut ledgreen1(D0), ledgreen2(D3), ledyellow1(PTC12), ledyellow2(D11), ledred1(D14), ledred2(D15);
volatile bool control_go = false, sample = false, sample_error, sample_error_strike; // EMG : booleans for controlling sample time of moving average and emg signal
volatile bool looptimerflag; // CONTROLLER : boolean that controls the sample time of the whole controller
double e30, e29, e28, e27, e26, e25, e24, e23, e22, e21, e20, // ACTION : in the action mechanism these variables calculate a current moving average error
e19, e18, e17, e16, e15, e14, e13, e12, e11, e10, e9,
e8, e7, e6, e5, e4, e3, e2, e1, er, error_count, error_turn_average, error_strike_average;
AnalogIn input1(A0), input2(A1); // EMG : Two AnalogIn EMG inputs, input1 (Left bicep), input2 (Right bicep)
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,// EMG : Left/Right bicep moving average memory variables (moving average is calculated over ten values)
Sample_EMG_L_7, Sample_EMG_L_8, Sample_EMG_L_9, Sample_EMG_L_10, moving_average_left;
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;
double minimum_L, maximum_L, EMG_L_min, EMG_L_max; // EMG CALIBRATION: variables that are used during the EMG calibration
double minimum_R, maximum_R, EMG_R_min, EMG_R_max;
double EMG_left_Bicep, EMG_Left_Bicep_filtered,
EMG_Left_Bicep_filtered_notch_1, EMG_Right_Bicep_filtered_notch_1;
double EMG_Right_Bicep, EMG_Left_Bicep_filtered_notch_2,
EMG_Right_Bicep_filtered_notch_2, EMG_Right_Bicep_filtered;
double Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, // EMG ACTION: variables to witch the threshold values calculated after the get asigned to
Threshold_Bicep_Right_1, Threshold_Bicep_Right_2;
double n=0; double c=0; double k=0; double p=0; double a=0; int HA=0; double g=0; // FLOW : these flow variables are assigned to certain values through out the script values in order to regulate the flow of the script
// FILTERS EMG
const double low_b0 = 0.05892937945281792 , low_b1 = 0.11785875890563584 , low_b2 = 0.05892937945281792, // Notch 1 LOW : VIA online biquad calculator Lowpass 520-48-0.7071-6
low_a1 = -1.205716572226748 , low_a2 = 0.44143409003801976 ;
const double high_b0 = 0.6389437261127494 , high_b1 = -1.2778874522254988 , high_b2 = 0.6389437261127494, // Notch 2 HIGH: VIA online biquad calculator Highpass 520-52-0.7071-6
high_a1 = -1.1429772843080923 , high_a2 = 0.41279762014290533 ;
const double high_b0_HP = 0.84855234544818812 , high_b1_HP = -1.6970469089637623 , high_b2_HP = 0.8485234544818812, // HIGHPASS : >25Hz sample rate 512Hz
high_a1_HP = -1.6564788473046559 , high_a2_HP = 0.7376149706228688 ;
const double low_b0_LP = 0.0013067079602315681, low_b1_LP = 0.0026134159204631363, low_b2_LP = 0.0013067079602315681, // LOWPASS : <5-10 Hz sample rate 512Hz
low_a1_LP = -1.9238184798980429 , low_a2_LP = 0.9290453117389691 ;
//Left bicep Filters
biquadFilter highpassfilter_1(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP); // EMG :
biquadFilter notchL1(high_a1, high_a2, high_b0, high_b1, high_b2); // EMG :
biquadFilter notchL2(low_a1, low_a2, low_b0, low_b1, low_b2); // EMG :
biquadFilter lowpassfilter_1(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP);
// Right bicep Filters
biquadFilter highpassfilter_2(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP); // EMG :
biquadFilter notchR1(high_a1, high_a2, high_b0, high_b1, high_b2); // EMG :
biquadFilter notchR2(low_a1, low_a2, low_b0, low_b1, low_b2); // EMG :
biquadFilter lowpassfilter_2(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP); // EMG :
// MOTORS
QEI motor_turn(D12,D13,NC,32); QEI motor_strike(D9,D10,NC,32); // TURN - STRIKE : Encoder for motor
PwmOut pwm_motor_turn(D5); PwmOut pwm_motor_strike(D6); // TURN - STRIKE : Pwm for motor
DigitalOut motordirection_turn(D4); DigitalOut motordirection_strike(D7); // TURN - STRIKE : Direction of the motor
double integrate_error_turn=0, previous_error_turn=0; double integrate_error_strike=0, previous_error_strike=0; // TURN - STRIKE : previous error and integration error motor
double position_turn, error_turn, reference_turn; double position_strike, error_strike, reference_strike;
double P_gain_turn; double I_gain_turn; double D_gain_turn; double P_gain_strike; double I_gain_strike; double D_gain_strike; // TURN - STRIKE : these gains get multiplied with the error inside the PID controller // TURN: these gains get multiplied with the error inside the PID controller
double pwm_motor_turn_P, pwm_motor_turn_I, pwm_motor_turn_D; double pwm_motor_strike_P, pwm_motor_strike_I, pwm_motor_strike_D; // TURN - STRIKE : variables that store the P, I and D action part of the error
double pwm_to_motor_turn; double pwm_to_motor_strike; // TURN - STRIKE : variable that is constructed by summing the values of the P, I and D action
const double cw=0; // MOTOR: turn direction zero is clockwise (front view)
const double ccw=1; // MOTOR: turn direction 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
double smp, f, pwm_strike;
//============================================================================================================================
// ___________________________
// // \\
// || [FUNCTIONS USED] ||
// \\___________________________//
void execute_plant_turn (); // TURN : Check error -> execute PID controller -> write pwm and direction to motor
void execute_plant_strike ();
double PID_control (double reference, double position, double &integrate_error,
double sample_time, double &previous_error,
double P_gain, double I_gain, double D_gain);
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
void countdown_from_5(); // PUTTY : 5 second countdown inside
void calibrate_motor(); // MOTOR : Calibrate starting position motor
void calibration(); // EMG : Calibrate the EMG signal (calculate min and max signal and determine threshold values)
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 red();void blue();void green();void white();void yellow();void cyan();void purple(); void black(); // DEBUG: Different color LEDS
void calibrate_potmeter(); // DEBUG/TEST : Calibration thresholds with potmeter
void calibrate_motor_EMG();
void Action_Controller(); // CONTROLLER : Decides and executes if the robot needs to turn to the left, right or strike based on the currently measured EMG signal
void led(double led1, double led2, double led3, double led4, double led5, double led6); // LED interface function (insert 1 if light on and 0 if light of) if (0,0,0,0,0,0) all leds are turned off
void leds_down_up(), led_up_down_up(), leds_down_up_min(), leds_down_up_max(); // LED functions
//============================================================================================================================
///////////////////////////////
// //
///////////////////////////////////////////// [MAIN FUNCTION] /////////////////////////////////////////////////////
// //
///////////////////////////////
//============================================================================================================================
int main() {
black(); // No LED active
pc.printf("Start of code \n\r");
led(1,1,1,1,1,1);
wait(2);
pc.baud(115200); // PC contactspeed : Set the baudrate
looptimer.attach(setlooptimerflag,(float)1/Fs); // CONTROLLER FLOW : Calls the looptimer flag every sample
blue();
calibration(); // EMG CALIBRATE : calibrate the EMG minimum and maximum values
red();
calibrate_motor(); // MOTOR CALIBRATE : The motor starting position (RED LED)
//calibrate_motor_EMG(); // MOTOR CALIBRATE : The motor starting position using EMG
//calibrate_potmeter(); // DEBUG/TEST : Calibration thresholds with potmeter
led(0,0,0,0,0,0);
wait (3); // REST before starting position
green(); // START CONTROLLOOP (GREEN LED)
Action_Controller(); // CONTROLLER : Decides and executes if the robot needs to turn to the left, right or strike based on the currently measured EMG signal
}
//============================================================================================================================
///////////////////////////////
// //
///////////////////////////////////////////// [FUNCTIONS DESCRIBED] /////////////////////////////////////////////////////
// //
///////////////////////////////
//============================================================================================================================
// ___________________________
// // \\
// || [TIME AND VALUE CONTROL] ||
// \\___________________________//
//
void setlooptimerflag(void) // Looptimerflag function
{ looptimerflag = true; }
void keep_in_range(double * in, double min, double max) // Put in certain min and max values that the input needs to stay within
{
*in > min ? *in < max? : *in = max: *in = min;
}
// FUNCTION 1 ___________________________
// // \\
// || [ACTIONCONTROLLER] ||
// \\___________________________//
void Action_Controller()
{
while (1)
{ // Start while loop
HA=0;
while(looptimerflag != true);
looptimerflag = false;
New_action: // Return here if action left, right or strike is executed
led(0,0,1,1,0,0); // Ready for new action (standby)
while(HA<175) // Measure EMG signal for 175 samples
{
if(looptimerflag==true)
{
looptimerflag=false;
sample_filter();
HA++;
}
}
HA=0;
// ___________________________
// : [Action 1: Turn Strike] :
// :___________________________:
//
if(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1) // Check if statement, if so then continue and start Strike action otherwise skip to next action
{ // Statement 1 start
blue(); n=0; k=0; p=0;
while(1)
{ // inf while loop strike until finished start
if (n==0) { // Change the reference point of the PID Strike controller
reference_strike=0; // Execute once (n is set to one and only gets executed if n equals zero)
n=1;
error_count=0;
}
if (looptimerflag == true) // Loop that executes the strike controller every sample (inside the controller the loudness is regulated)
{
looptimerflag=false;
activate_PID_Controller_strike(P_gain_strike, I_gain_strike, D_gain_strike);
execute_plant_strike();
}
if (fabs(position_strike)<2) // If the bottle is hit (hit if the position is greater than the set hit point) then initiate return
{ // Statement Return start
while(1)
{ // inf while loop return after strike start
yellow();
if(k==0) { // Change reference point of the PID Strike controller back to the original position
p=1;
reference_strike=Hit;
error_count=0;
k=1;
smp=0;
pc.printf("return \n\r");
}
if (looptimerflag == true) { // Loop that executes the strike controller every sample (loudness is deactivated by the value of p)
looptimerflag=false;
activate_PID_Controller_strike(P_gain_strike, I_gain_strike, D_gain_strike);
execute_plant_strike();
}
if (fabs(error_strike_average) < 2 && error_count>100) { // If error is small enough and at least 100 samples have passed since the return execute new action
yellow();
pc.printf("new action \n\r");
deactivate_PID_Controller(P_gain_strike, I_gain_strike, D_gain_strike);
execute_plant_strike();
Sample_EMG_L_10=0; Sample_EMG_L_9=0; Sample_EMG_L_8=0; Sample_EMG_L_7=0; Sample_EMG_L_6=0; Sample_EMG_L_5=0; Sample_EMG_L_4=0; Sample_EMG_L_3=0; Sample_EMG_L_2=0; Sample_EMG_L_1=0;
Sample_EMG_R_10=0; Sample_EMG_R_9=0; Sample_EMG_R_8=0; Sample_EMG_R_7=0; Sample_EMG_R_6=0; Sample_EMG_R_5=0; Sample_EMG_R_4=0; Sample_EMG_R_3=0; Sample_EMG_R_2=0; Sample_EMG_R_1=0;
goto New_action;
}
} // inf while loop return after strike end
} // Statement Return end
} // inf while loop strike until finished end
} // Statement 2 end
// ___________________________
// : [Action 2: Turn Left] :
// :___________________________:
//
if(moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1)
{
yellow(); n=0;
led(1,1,0,0,0,0); // Green
while(1)
{
if (n==0)
{
Change_Turn_Position_Left(reference_turn, change_one_bottle);
n=1; error_count=0;
}
if (looptimerflag == true)
{
looptimerflag=false;
activate_PID_Controller_turn(P_gain_turn, I_gain_turn, D_gain_turn);
execute_plant_turn();
}
if (fabs(error_turn_average) < 1 && error_count>250)
{
pc.printf("new action \n\r");
deactivate_PID_Controller(P_gain_turn, I_gain_turn, D_gain_turn);
execute_plant_turn();
Sample_EMG_L_10=0; Sample_EMG_L_9=0; Sample_EMG_L_8=0; Sample_EMG_L_7=0; Sample_EMG_L_6=0; Sample_EMG_L_5=0; Sample_EMG_L_4=0; Sample_EMG_L_3=0; Sample_EMG_L_2=0; Sample_EMG_L_1=0;
Sample_EMG_R_10=0; Sample_EMG_R_9=0; Sample_EMG_R_8=0; Sample_EMG_R_7=0; Sample_EMG_R_6=0; Sample_EMG_R_5=0; Sample_EMG_R_4=0; Sample_EMG_R_3=0; Sample_EMG_R_2=0; Sample_EMG_R_1=0;
goto New_action;
}
}
}
// ___________________________
// : [Action 3: Turn Right] :
// :___________________________:
//
// //Purple\\
if(moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Left_1)
{
purple(); n=0;
// pc.printf("Right \n\r");
led(0,0,0,0,1,1); // Red right
while(1)
{
if (n==0)
{
Change_Turn_Position_Right(reference_turn, change_one_bottle);
n=1; error_count=0;
}
if (looptimerflag == true)
{
looptimerflag=false;
activate_PID_Controller_turn(P_gain_turn, I_gain_turn, D_gain_turn);
execute_plant_turn();
}
if (fabs(error_turn_average) < tuning13 && error_count> tuning14)
{
pc.printf("new action \n\r");
deactivate_PID_Controller(P_gain_turn, I_gain_turn, D_gain_turn);
execute_plant_turn();
moving_average_right=0;
moving_average_left=0;
Sample_EMG_L_10=0; Sample_EMG_L_9=0; Sample_EMG_L_8=0; Sample_EMG_L_7=0; Sample_EMG_L_6=0; Sample_EMG_L_5=0; Sample_EMG_L_4=0; Sample_EMG_L_3=0; Sample_EMG_L_2=0; Sample_EMG_L_1=0;
Sample_EMG_R_10=0; Sample_EMG_R_9=0; Sample_EMG_R_8=0; Sample_EMG_R_7=0; Sample_EMG_R_6=0; Sample_EMG_R_5=0; Sample_EMG_R_4=0; Sample_EMG_R_3=0; Sample_EMG_R_2=0; Sample_EMG_R_1=0;
goto New_action;
}
}
}
}
}
// ___________________________
// // \\
// || [CALIBRATE] ||
// \\___________________________//
// Calibrate starting postion (RED LED)
// ___________________________
// : [Starting position motor] :
// :___________________________:
//
void calibrate_motor()
{
pc.printf("Button calibration \n\r");
while(1) {
red();// RED LED
if (buttonL2.read() < 0.7) {
motordirection_turn = cw;
pwm_motor_turn = 0.1;
led(0,0,0,0,1,1);
}
if (buttonL1.read() < 0.7) {
motordirection_turn = ccw;
pwm_motor_turn = 0.1;
led(1,1,0,0,0,0);
}
led(0,0,0,0,0,0);
pwm_motor_turn = 0;
if (buttonH1.read() < 0.7) {
motordirection_strike = cw;
pwm_motor_strike = 0.02;
led(0,0,0,0,1,1);
}
if (buttonH2.read() < 0.7) {
motordirection_strike = ccw;
pwm_motor_strike = 0.02;
led(1,1,0,0,0,0);
}
pwm_motor_strike = 0;
if ((buttonL2.read() < 0.8) && (buttonL1.read() < 0.8)) { // 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
goto calibration_starting_position_complete3; // Calibration complete (exit while loop)
}
}
calibration_starting_position_complete3:;
while(1) {
// go to starting position
if(k==0) { // Change reference point of the PID Strike controller back to the original position
p=1;
reference_strike=Hit;
error_count=0;
k=1;
smp=0;
led(1,1,0,0,1,1);
}
if (looptimerflag == true) { // Loop that executes the strike controller every sample (loudness is deactivated by the value of p)
looptimerflag=false;
activate_PID_Controller_strike(P_gain_strike, I_gain_strike, D_gain_strike);
execute_plant_strike();
}
if (fabs(error_strike_average) < 2 && error_count>100) { // If error is small enough and at least 100 samples have passed since the return execute new action
// pc.printf("starting point calibration completed \n\r");
deactivate_PID_Controller(P_gain_strike, I_gain_strike, D_gain_strike);
execute_plant_strike();
goto calibration_position_strike_complete;
}
} // inf while loop return after strike end
calibration_position_strike_complete:
;
}
// ___________________________
// // \\
// || [EMG_Calibration] ||
// \\___________________________//
//
void calibration()
{
// [MINIMUM VALUE BICEPS CALIBRATION] //
countdown_from_5();
c=0;
while(c<2560) // 512Hz -> 2560 is equal to five seconds
{
leds_down_up_min();
Filter(); // Filter EMG signal
minimum_L=fabs(EMG_Left_Bicep_filtered)+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
minimum_R=fabs(EMG_Right_Bicep_filtered)+minimum_R;
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);
led(0,0,0,0,0,0);
wait (2); //cooldown
// [MAXIMUM VALUE BICEPS CALIBRATION] //
countdown_from_5();
c=0;
while(c<2560) // 512Hz -> 2560 is equal to five seconds
{
leds_down_up_max();
Filter(); // Filter EMG signal
maximum_L=fabs(EMG_Left_Bicep_filtered)+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
maximum_R=fabs(EMG_Right_Bicep_filtered)+maximum_R;
c++; // Every sample c is increased by one until the statement c<2560 is false
wait(0.001953125);
}
// 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);
led(0,0,0,0,0,0);
wait (2); //cooldown
// [ THRESHOLD CALCULATION ] //
Threshold_Bicep_Left_1=(fabs(EMG_L_max-EMG_L_min)*0.45)+fabs(EMG_L_min); // LEFT EMG: value at which the contraction of the muscle is at 45% compared to the maximum and minimum contraction of the muscle;
Threshold_Bicep_Left_2=(fabs(EMG_L_max-EMG_L_min)*0.9)+fabs(EMG_L_min); // LEFT EMG: Threshold at 90% of the maximum contraction
Threshold_Bicep_Right_1=(fabs(EMG_R_max-EMG_R_min)*0.45)+fabs(EMG_R_min); // RIGHT EMG: Threshold at 45% of the maximum contraction
Threshold_Bicep_Right_2=(fabs(EMG_R_max-EMG_R_min)*0.9)+fabs(EMG_R_min); // RIGHT EMG: Threshold at 90% of the maximum contraction
// 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);
}
// ___________________________
// // \\
// || [TURN PLANT] ||
// \\___________________________//
void execute_plant_turn()
{
if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200)) // If value exceeds -4200 and 4200 (number of counts equal to one revolution) than reset to zero
{ motor_turn.reset(); }
position_turn = conversion_counts_to_degrees * motor_turn.getPulses(); // Convert counts to degrees
double pwm_to_motor_turn = PID_control(reference_turn, position_turn, integrate_error_turn, sample_time, previous_error_turn, P_gain_turn, I_gain_turn, D_gain_turn);
// Execute PID controller and calculate the pwm to put to the motor
keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to the plant but make sure the max and min pwm put to the plant stays between -1 and 1
pc.printf("pwm %f \n\r", pwm_to_motor_turn); // LINE FOR TESTING
if(pwm_to_motor_turn > 0) // Check error and decide the direction the motor has to turn
{ motordirection_turn=ccw;}
else
{ motordirection_turn=cw; }
pwm_motor_turn=(abs(pwm_to_motor_turn)); // Put the absolute value of the PID controller to the pwm (negative pwm does not work)
sample_filter(); // What is the current EMG value
if(sample_error) // sample_error -- e10;e9;e8;e7;e6;e5:e4;e3;e2;e1 -- error_turn_average --- er
{
sample_error=false;
e1 = fabs(position_turn - reference_turn);
e30=e29; e29=e28 ;e28=e27; e27=e26; e26=e25; e25=e24; e24=e23; e23=e22; e22=e21; e21=e20;
e20=e19; e19=e18 ;e18=e17; e17=e16; e16=e15; e15=e14; e14=e13; e13=e12; e12=e11; e11=e10;
e10=e9 ;e9=e8; e8=e7; e7=e6; e6=e5; e5=e4; e4=e3; e3=e2; e2=e1;
}
error_turn_average=(e1+e2+e3+e4+e5+e6+e7+e8+e9+e10+e11+e12+e13+e14+e15+e16+e17+e18+e19+e20+e21+e22+e23+e24+e25+e26+e27+e28+e29+e30)/30;
er++;
error_count++;
}
// ___________________________
// // \\
// || [STRIKE PLANT] ||
// \\___________________________//
//
void execute_plant_strike()
{
if ((motor_strike.getPulses()>4200) || (motor_strike.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
{
motor_strike.reset();
pc.printf("RESET \n\r");
}
position_strike = conversion_counts_to_degrees * motor_strike.getPulses();
double pwm_to_motor_strike=PID_control(reference_strike, position_strike, integrate_error_strike, sample_time, previous_error_strike, P_gain_strike, I_gain_strike, D_gain_strike);
keep_in_range(&pwm_to_motor_strike, -1,1); // Pass to motor controller but keep it in range!
if(pwm_to_motor_strike > 0) // Check error and decide direction to turn
{ motordirection_strike=cw; }
else
{ motordirection_strike=ccw; }
if(p==1)
{ pwm_motor_strike=fabs(pwm_to_motor_strike); } // Return strike mechanism to original position
if(p==0) // p is put to one if the strike has been executed and the stick needs to return to the original position
{ // PWM voor slaan
while(smp<128)
{
if(looptimerflag == true)
{
looptimerflag=false;
sample_filter();
double sum_muscle_force=(moving_average_left+moving_average_right)/(EMG_L_max+EMG_R_max);
keep_in_range(&sum_muscle_force, 0,1);
if (sum_muscle_force < tuning18) { led(0,0,0,0,0,0) ; pwm_strike=tuning24; }
if (sum_muscle_force > tuning18) { led(1,0,0,0,0,0) ; pwm_strike=tuning25; }
if (sum_muscle_force > tuning19) { led(1,1,0,0,0,0) ; pwm_strike=tuning26; }
if (sum_muscle_force > tuning20) { led(1,1,1,0,0,0) ; pwm_strike=tuning27; }
if (sum_muscle_force > tuning21) { led(1,1,1,1,0,0) ; pwm_strike=tuning28; }
if (sum_muscle_force > tuning22) { led(1,1,1,1,1,0) ; pwm_strike=tuning29; }
if (sum_muscle_force > tuning23) { led(1,1,1,1,1,1) ; pwm_strike=tuning30; }
smp++;
if(smp==127)
{ pwm_motor_strike=abs(pwm_strike); }
}
}
}
sample_filter(); // Measure current EMG
if(sample_error_strike)
{
sample_error_strike=false;
e1 = fabs(position_strike - reference_strike);
e30=e29; e29=e28 ;e28=e27; e27=e26; e26=e25; e25=e24; e24=e23; e23=e22; e22=e21; e21=e20;
e20=e19; e19=e18 ;e18=e17; e17=e16; e16=e15; e15=e14; e14=e13; e13=e12; e12=e11; e11=e10;
e10=e9 ;e9=e8; e8=e7; e7=e6; e6=e5; e5=e4; e4=e3; e3=e2; e2=e1;
}
error_strike_average=(e1+e2+e3+e4+e5+e6+e7+e8+e9+e10+e11+e12+e13+e14+e15+e16+e17+e18+e19+e20+e21+e22+e23+e24+e25+e26+e27+e28+e29+e30)/30;
er++;
error_count++;
}
// ___________________________
// // \\
// || [PID CONTROLLER] ||
// \\___________________________//
//
double PID_control(double reference, double position, double &integrate_error, double sample_time, double &previous_error, double P_gain, double I_gain, double D_gain)
{
double error=(reference - position); // Current error (input controller)
integrate_error=integrate_error_turn + sample_time*error_turn; // Integral error output
// overwrite previous integrate error by adding the current error
// multiplied by the sample time.
double error_derivative=(error - previous_error)/sample_time; // Derivative error output
error_derivative=Lowpassfilter_derivative.step(error_derivative); // 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
double pwm_motor_P = error*P_gain; // Output P controller to pwm
double pwm_motor_I = integrate_error*I_gain; // Output I controller to pwm
double pwm_motor_D = error_derivative*D_gain; // Output D controller to pwm
double pwm_to_motor = pwm_motor_P + pwm_motor_I + pwm_motor_D;
return pwm_to_motor;
}
// ___________________________
// // \\
// || [SAMPLE] ||
// \\___________________________//
//
void sample_filter()
{
Filter();
take_sample();
if(sample)
{
sample=false;
Sample_EMG_L_1 = fabs(EMG_Left_Bicep_filtered); Sample_EMG_R_1 = fabs(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++;
}
// ___________________________
// // \\
// || [ FILTER ] ||
// \\___________________________//
//
void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
{
EMG_left_Bicep = input1; EMG_Right_Bicep = input2; // Input EMG unfiltered
EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep); // Highpass filter
EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered); // Rectify
EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered); // Lowpass filter
}
// ___________________________
// // \\
// || [TAKE SAMPLE] ||
// \\___________________________//
//
void take_sample() // Take a sample every 4th sample for moving average, every 5th sample for average error turn and every sample for average error strike
{
if(n==4)
{sample = true; n=0;}
if(er==5)
{sample_error = true; er=0;}
sample_error_strike = true;
}
// ___________________________
// // \\
// || [CHANGE REFERENCE] ||
// \\___________________________//
//
void Change_Turn_Position_Right (double& reference, double change_one_bottle)
{
if(reference==2*change_one_bottle) // If reference value at right boundary bottle and function is executed than immediatly turn 5 bottles to the left (ref to -90)
{ reference=-2*change_one_bottle; }
else
{ reference = reference + change_one_bottle;
keep_in_range(&reference, -2*change_one_bottle, 2*change_one_bottle); } // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
}
void Change_Turn_Position_Left (double& reference, double change_one_bottle)
{
if(reference==-2*change_one_bottle) // If reference value at left boundary bottle and function is executed than immediatly turn 5 bottles to the left (ref to +90)
{ reference=2*change_one_bottle; }
else
{ reference = reference - change_one_bottle;
keep_in_range(&reference, -2*change_one_bottle, 2*change_one_bottle); }
}
// ____________________________
// // \\
// | [(DE)ACTIVATE PID CONTROLLERS] |
// \\____________________________//
//
void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain)
{
P_gain=0; I_gain=0; D_gain=0; // Deactivating values of PID controller
pwm_motor_turn=0; pwm_motor_strike=0;
}
void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
{
P_gain_turn=tuning4;
I_gain_turn=tuning5;
D_gain_turn=tuning6;
}
void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
{
P_gain_strike=tuning1;
I_gain_strike=tuning2;
D_gain_strike=tuning3;
}
// ____________________________
// // \\
// || [LED INTERFACE FUNCTIONS] ||
// \\____________________________//
//
void led(double led1, double led2, double led3, double led4, double led5, double led6)
{ledgreen1 = led1; ledgreen2 = led2; ledyellow1 = led3; ledyellow2 = led4; ledred1 = led5; ledred2 = led6;}
void leds_down_up_min()
{
if(a==15) {led(1,1,0,0,0,0);}
if(a==30) {led(1,1,1,0,0,0);}
if(a==45) {led(1,1,0,1,0,0);}
if(a==60) {led(1,1,0,0,1,0);}
if(a==75) {led(1,1,0,0,0,1); a=0;}
a++;
}
void leds_down_up_max()
{
if(a==15) {led(0,0,0,0,1,1);}
if(a==30) {led(1,0,0,0,1,1);}
if(a==45) {led(0,1,0,0,1,1);}
if(a==60) {led(0,0,1,0,1,1);}
if(a==75) {led(0,0,0,1,1,1); a=0;}
a++;
}
void countdown_from_5() // Countdown from 6 till 0 with LED(interface)
{
led(1,1,1,1,1,1);
wait(1);
led(1,1,1,1,1,0);
wait(1);
led(1,1,1,1,0,0);
wait(1);
led(1,1,1,0,0,0);
wait(1);
led(1,1,0,0,0,0);
wait(1);
led(1,0,0,0,0,0);
wait(1);
}
// _______________________________________
// // \\
// || [UNUSED/TESTING/CONCEPTUAL FUNCTIONS] ||
// \\_______________________________________//
//
// UNUSED/TESTING/CONCEPTUAL FUNCTIONS
void red() { debug_led_red=on; debug_led_blue=off; debug_led_green=off; }
void blue() { debug_led_red=off; debug_led_blue=on; debug_led_green=off; }
void green() { debug_led_red=off; debug_led_blue=off; debug_led_green=on; }
void white() { debug_led_red=on; debug_led_blue=on; debug_led_green=on; }
void yellow() { debug_led_red=on; debug_led_blue=off; debug_led_green=on; }
void cyan() { debug_led_red=off; debug_led_blue=on; debug_led_green=on; }
void purple() { debug_led_red=on; debug_led_blue=on; debug_led_green=off; }
void black() { debug_led_red=off; debug_led_blue=off; debug_led_green=off; }
void calibrate_potmeter() // DEBUG/TEST: Calibration thresholds with potmeter (testing tool without EMG)
{
EMG_L_max = 100;
EMG_L_min = 0;
EMG_R_max = 100;
EMG_R_min = 0;
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
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);
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
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);
}
// void calibrate_motor_EMG()
// // CALIBRATE STRIKE MOTOR WITH EMG
//while(1)
//{
// led(0,0,0,0,0,0);
//
// if(looptimerflag==true)
// {
// sample_filter();
// pc.printf("%f %f \n\r", moving_average_left, moving_average_right);
// pc.printf("%f \n\r", pwm_motor_strike);
//
//while (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1) // Links
//{
//if(looptimerflag==true)
// {
// sample_filter();
//motordirection_strike = ccw;
// pwm_motor_strike = 0.08;
// led(1,1,0,0,0,0);
// }}
//
//while (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Left_1) // Rechts
//{ if(looptimerflag==true)
// {
// sample_filter();
//motordirection_strike = cw;
// pwm_motor_strike = 0.08;
// led(0,0,0,0,1,1);} }
//
// pwm_motor_strike=0;
// led(0,0,0,0,0,0);
//
//if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left > Threshold_Bicep_Left_2) // Beide
//{ motor_strike.reset();
// reference_strike=0;
// led(1,1,1,1,1,1);
// wait(1);
// moving_average_right=0;
// moving_average_left=0;
// g=0;
// goto calibration_starting_position_complete;}
// }
// calibration_starting_position_complete:;
//
// while(g<512)
// {
// if(looptimerflag==true)
// {
// sample_filter();
// g++;
// }
// }
//
//// CALIBRATE TURN MOTOR WITH EMG
// while(1)
// {
// led(0,0,0,0,0,0);
//
// if(looptimerflag==true)
// {
// sample_filter();
// pc.printf("%f %f \n\r", moving_average_left, moving_average_right);
//
//while (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1) // Links
//{
//if(looptimerflag==true)
// {
// sample_filter();
//motordirection_turn = ccw;
// pwm_motor_turn = 0.02;
// led(1,1,0,0,0,0);}}
//
//while (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Left_1) // Rechts
//{ if(looptimerflag==true)
// {
// sample_filter();
//motordirection_turn = cw;
// pwm_motor_turn = 0.02;
// led(0,0,0,0,1,1);} }
//
// pwm_motor_strike=0;
// led(0,0,0,0,0,0);
//
//if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left > Threshold_Bicep_Left_2) // Beide
//{ motor_strike.reset();
// reference_turn=0;
// led(1,1,1,1,1,1);
// wait(1);
// moving_average_right=0;
// moving_average_left=0;
// g=0;
// goto calibration_starting_position_complete2;}
// }
// }
// }
// calibration_starting_position_complete2:;
//
// while(g<512)
// {
// if(looptimerflag==true)
// {
// sample_filter();
// g++;
// }
// }
//
// wait(2);
// led(0,0,0,0,0,0);
// wait(1);
// led(1,0,1,0,1,0);
// wait(1);
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 08 Oct 2015 |
| Imports: | 4 |
| Forks: | 0 |
| Commits: | 111 |
| Dependents: | 0 |
| Dependencies: | 6 |
| Followers: | 4 |
