Thomas Burgers
/
ZZ-TheChenneRobot
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Encoder HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp
- Committer:
- ThomasBNL
- Date:
- 2015-10-24
- Revision:
- 79:5c4b13d4fe7e
- Parent:
- 78:bcf0b5627b47
- Child:
- 80:413cb8a1b919
File content as of revision 79:5c4b13d4fe7e:
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
//////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
/////// ////// ////// ////// ////// /////// ////// /////// ////// /////////////////////////////////////////////|
/////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
/////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
/////// ///////////////// ////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
/////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
/////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
/////// ///////////////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
/////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
/////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
/////// ////// ////// ////// ////// ////// ////// ////// ////// /////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
/////// /////////////// ///////////// /////// ////// /////////////////////////////////////////////////////////////////////////|
/////// ///////////// //////////// ///// ////// ////////////////////////////////////////////////////////////////////////|
/////// ////// /////////// /////////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// ////////// // ////////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ///////// //// ///////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// //////// ////// //////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// /////// /////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// ////// ////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
/////// ////// //////////// ///// ///// ////// ////////////////////////////////////////////////////////////////////////|
/////// ////// ////////////// //// ////// ////// /////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
// Groep 12: The Chenne Band //
// ___________________________
// // \\
// || [Table of content] ||
// \\___________________________//
//
// Introduction
// Libraries.................................................
//
//
//
//
//============================================================================================================================
// ___________________________
// // \\
// || [Libraries] ||
// \\___________________________//
//
#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "biquadFilter.h"
#include "encoder.h"
//============================================================================================================================
// ___________________________
// // \\
// || [Tuning Device] ||
// \\___________________________//
//
// PID TUNING
const double tuning1=0.0009995; // P_gain_strike value
const double tuning2=0; // I_gain_strike value
const double tuning3=0.0000055; // D_gain_strike value
const double tuning4=0.01769; // P_gain_turn value
const double tuning5=0; // I_gain_turn value
const double tuning6=0.00000700; // D_gain_turn value
// MOVEMENT ROBOT TUNING
const double change_one_bottle=30; // Number of degrees between two bottles
const double Hit=30; // Number of ccw degrees in comparison to starting position when bottle is hit
// EMG TUNING
const double tuning7=0; // 0: Read potmeter value as if EMG signal
// 1: Measure EMG value to control action controller
const double tuning32=0.8; // If tuning 7=0 -> pwm striking speed
const double tuning8=0; // 0: Deactivate EMG Calibration
// 1: Activate EMG Calibration (Put tuning 9, 13, 15 ... to 0)
const double tuning16=0.2; // percentage at which the first threshold is put for both biceps (value between 0 and 1) (eg 0.2 equals 20%)
const double tuning17 =0.6; // percentage at which the first threshold is put for both biceps (value between 0 and 1)
// STRIKE TUNING
const double tuning9=1; // 0: Nothing
// 1: Test Strike mechanism (no need to put Potmeter to certain value)
const double tuning10=0.5; // how small (degrees) does the error strike average need to be before continuing
const double tuning11=100; // how many samples at least before strike error is small enough
const double tuning24=0.03; // if <smaller than tuning18 than execute tuning 24 to prevent being stuck inside this loop (minimum pwm)
const double tuning18=0.1, tuning25=0.06; // First value: How much muscle percentage force is required (between 0-1)
const double tuning19=0.2, tuning26=0.09; // 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.18;
const double tuning21=0.5, tuning28=0.35;
const double tuning22=0.7, tuning29=0.70;
const double tuning23=0.9, tuning30=1;
const double tuning31=0; // 0: Real time muscle force value is used for strike loudness selection
// 1: average muscle force value is used for strike loudness selection
// TURN LEFT
const double tuning12=0; // 0: Nothing
// 1: Test Turning Left mechanism (no need to put Potmeter to certain value)
const double tuning13=1; // error turn average smaller than (ENTRY) degrees than continue (BOTH FOR TURN LEFT AND TURN ACTION!
const double tuning14=250; // how many samples at least before strike error is small enough (BOTH FOR TURN LEFT AND TURN ACTION!
// TURN RIGHT
const double tuning15=0; // 0: Nothing
// 1: Test Turning Right mechanism (no need to put Potmeter to certain value)
// SEE tuning 13 and 14
// WAIT STATEMENTS BIJ STRIKING, LEFT and RIGHT KORTER OF LANGER (regel 359, 427, 473-478)
// LINE 978+979 => REPLACE ALL CURRENTLY USED LED FUNCTIONS WITH THIS COMPACT NOTATION IF IT WORKS ( 1=ON, 0=OFF )
//============================================================================================================================
// ___________________________
// // \\
// || [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 calibration 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; // 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 : NOG OPZOEKEN!! : >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 : NOG OPZOEKEN!! <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 : moeten nog waardes voor gemaakt worden? (>25Hz doorlaten)
biquadFilter notchL1(high_a1, high_a2, high_b0, high_b1, high_b2); // EMG : moeten nog waardes voor gemaakt worden (>52Hz doorlaten)
biquadFilter notchL2(low_a1, low_a2, low_b0, low_b1, low_b2); // EMG : moeten nog waardes voor gemaakt worden (<48Hz doorlaten)
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 : moeten nog waardes voor gemaakt worden?
biquadFilter notchR1(high_a1, high_a2, high_b0, high_b1, high_b2); // EMG : moeten nog waardes voor gemaakt worden
biquadFilter notchR2(low_a1, low_a2, low_b0, low_b1, low_b2); // EMG : moeten nog waardes voor gemaakt worden
biquadFilter lowpassfilter_2(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP); // EMG : moeten nog waardes voor gemaakt worden
// 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
// FILTER: D-action
const double a0 = 0.000021080713160785432, a1 = 0.000042161426321570865, a2 = 0.000021080713160785432, b1 = -1.990754082898736, b2 = 0.9908384057513788; //(0.75Hz)
biquadFilter Lowpassfilter_derivative(b1,b2,a0,a1,a2); // BiquadFilter used for the filtering of the Derivative action of the PID-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 double ledon = 1; //
const double ledoff = 0; //
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
void led12(), led23(), led34(), led45(), led56(), led61(), led1(), led2(), led3(), led4(), led5(), led6(), led_off(), led_on();
void leds_down_up(), led_up_down_up();
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 ControlGo(); // EMG : function that gets a ticker attached and sets a certain loop to true every sample
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 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);
//============================================================================================================================
///////////////////////////////
// //
///////////////////////////////////////////// [MAIN FUNCTION] /////////////////////////////////////////////////////
// //
///////////////////////////////
//============================================================================================================================
int main() {
black(); // No LED active
pc.printf("Start of code \n\r");
pc.baud(115200); // PC contactspeed : Set the baudrate
red();
calibrate_motor(); // MOTOR CALIBRATE : The motor starting position (RED LED)
blue();
if (tuning8 == 1)
{
calibration(); // EMG CALIBRATE : calibrate the EMG minimum and maximum values
}
calibrate_potmeter(); // DEBUG/TEST : Calibration thresholds with potmeter
looptimer.attach(setlooptimerflag,(float)1/Fs); // CONTROLLER FLOW : Calls the looptimer flag every sample
black();
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] /////////////////////////////////////////////////////
// //
///////////////////////////////
//============================================================================================================================
// FUNCTION 1 ___________________________
// // \\
// || [ACTIONCONTROLLER] ||
// \\___________________________//
void Action_Controller()
{
while (1)
{ // Start while loop
while(looptimerflag != true);
looptimerflag = false;
Nieuwe_actie: // Return here if action left, right or strike is executed
green(); // GREEN LED: ready to fire again
led34();
wait(2);
if(tuning7 == 1)
{
sample_filter(); // What is the current EMG value
}
if(tuning7 == 0)
{
// POTMETER: SIMULATE EMG SIGNAL
moving_average_left = (input1.read())*100; // EMG Right bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
moving_average_right = (input2.read())*100; // EMG Left bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
pc.printf("mov_r = %f, mov_l = %f \n\r", moving_average_right, moving_average_left);
}
// ___________________________
// : [Action 1: Turn Strike] :
// :___________________________:
//
// Blue (strike) - Yellow (return)
if(tuning9 == 1)
{
// TEMPORARY: TO TEST STRIKE MECHANISM
moving_average_left = Threshold_Bicep_Right_1+1; // TIJDELIJK PID TEST
moving_average_right = Threshold_Bicep_Left_1+1; // TIJDELIJK PID TEST
}
if (moving_average_right > Threshold_Bicep_Right_1 && moving_average_left > Threshold_Bicep_Left_1) // Statement 1 (if both are satisfied execute)
{ // Statement 1 start
blue(); n=0; k=0; p=0;
pc.printf("Slag \n\r");
wait(0.5); // TIJDELIJK??
if(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1) // Check if statement 1 is still true then continue and start Strike
{ // Statement 2 start
while(1)
{ // inf while loop strike until finished start
if (n==0) // Change the reference point of the PID Strike controller
{ reference_strike=Hit; n=1; error_count=0; } // Execute once (n is set to one and only gets executed if n equals zero)
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)>Hit) // 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=0; error_count=0; k=1;
pc.printf("return \n\r");
}
//pc.printf("ref_t = %f, e_cnt= %f e_av=%f \n\r k=%f, er_cnt= %f", reference_strike, error_strike, error_strike_average, k, error_count); // LINE USED FOR TESTING
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();
}
printf(" %f \n\r",error_strike_average); // LINE USED FOR TESTING
if (fabs(error_strike_average) < tuning10 && error_count > tuning11) // 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();
goto Nieuwe_actie;
}
} // inf while loop return after strike end
} // Statement Return end
} // inf while loop strike until finished end
} // Statement 2 end
} // Statement 1 end
// ___________________________
// : [Action 2: Turn Left] :
// :___________________________:
//
// //Yellow\\
if(tuning12 == 1)
{
// TEMPORARY: TO TEST TURN LEFT MECHANISM
moving_average_left = Threshold_Bicep_Left_2+1; // TIJDELIJK PID TEST
moving_average_right = Threshold_Bicep_Right_1-1; // TIJDELIJK PID TEST
}
if (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1)
{
yellow(); n=0;
pc.printf("LEFT \n\r");
led12();
wait(0.5); // TIJDELIJK
while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right < Threshold_Bicep_Right_1)
{
if (n==0)
{
Change_Turn_Position_Left(reference_turn, change_one_bottle);
n=1; error_count=0;
}
// pc.printf("ref_t = %f, e_cnt= %f e_av=%f \n\r", reference_turn, error_count, error_turn_average); // LINE USED FOR TESTING
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();
goto Nieuwe_actie;
}
}
}
// ___________________________
// : [Action 3: Turn Right] :
// :___________________________:
//
// //Purple\\
if(tuning15 == 1)
{
// TEMPORARY: TO TEST TURN LEFT MECHANISM
moving_average_left = Threshold_Bicep_Left_1-1; // TIJDELIJK PID TEST
moving_average_right = Threshold_Bicep_Right_2+1; // TIJDELIJK PID TEST
}
if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Left_1)
{
purple(); n=0;
pc.printf("Right \n\r");
led_on();
wait(0.5);
led_off();
wait(0.5);
led_on();
wait(0.5); // TIJDELIJK
led_off();
while(moving_average_right > Threshold_Bicep_Right_1 && moving_average_left < Threshold_Bicep_Left_1)
{
if (n==0)
{
Change_Turn_Position_Right(reference_turn, change_one_bottle);
n=1; error_count=0;
}
// pc.printf("ref_t = %f, e_cnt= %f e_av=%f \n\r", reference_turn, error_count, error_turn_average); // LINE USED FOR TESTING
if (looptimerflag == true)
{
looptimerflag=false;
activate_PID_Controller_turn(P_gain_turn, I_gain_turn, D_gain_turn);
execute_plant_turn();
led_up_down_up();
}
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();
goto Nieuwe_actie;
}
}
}
}
}
// ___________________________
// // \\
// || [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.5)
{ motordirection_turn = cw;
pwm_motor_turn = 0.2;
led56();
}
if (buttonL1.read() < 0.5)
{ motordirection_turn = ccw;
pwm_motor_turn = 0.2;
led12();
}
pwm_motor_turn = 0;
led_off();
if (buttonH1.read() < 0.5)
{ motordirection_strike = cw;
pwm_motor_strike = 0.02;
led56();
}
if (buttonH2.read() < 0.5)
{ motordirection_strike = ccw;
pwm_motor_strike = 0.02;
led12();
}
pwm_motor_strike = 0;
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
goto calibration_starting_position_complete; // Calibration complete (exit while loop)
}
}
calibration_starting_position_complete:;
}
// ___________________________
// // \\
// || [EMG_Calibration] ||
// \\___________________________//
//
void calibration()
{
// ___________________________________
// : [Minimum value bicep calibration] :
// :___________________________________:
blue();
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;
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 bicep 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;
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);
wait (3); // Cooldown
// _______________________________________________
// : [THRESHOLDS CALCULATION FROM MIN AND MAX EMG] :
// :_______________________________________________:
//
const float Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*tuning16)+EMG_L_min;; // EMG LEFT: Threshold put at 20% of the EMG range
const float Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*tuning17)+EMG_L_min; // EMG LEFT: Threshold put at 60% of the EMG range
const float Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*tuning16)+EMG_R_min; // EMG RIGHT: Threshold put at 20% of the EMG range
const float Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*tuning17)+EMG_R_min; // EMG RIGHT: Threshold put at 60% of the EMG range
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)
if(tuning7 == 1)
{
sample_filter(); // What is the current EMG value
}
if (tuning7 ==0)
{
take_sample(); // TEMPORARY -> use sample_filter() normally
}
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) // p is put to one if return action is put to active
{ pwm_motor_strike=(abs(pwm_to_motor_strike));
smp=0; }
//pc.printf("mov_r = %f, mov_l = %f, pwm_strike = %f position = %f \n\r", moving_average_right, moving_average_left, pwm_strike, position_strike); // LINE FOR TESTING
if(p==0) // STRIKING MECHANISM
{
if (tuning7 ==0) // UNACTIVE EMG
{
pwm_motor_strike=tuning32;
take_sample();
}
if(tuning7 == 1) // ACTIVE EMG
{
sample_filter(); // What is the current EMG value
double signal_above_threshold=(moving_average_left+moving_average_right);
double max_signal=(EMG_L_max+EMG_R_max);
double sum_muscle_force=signal_above_threshold/max_signal;
keep_in_range(&sum_muscle_force, 0,1);
if(tuning31==1)
{double sum_muscle_force_avg=(sum_muscle_force+sum_muscle_force_avg)/f;}
if(tuning31==0)
{double sum_muscle_force_avg=sum_muscle_force;
pc.printf("force=%f \n\r", sum_muscle_force_avg);
if (sum_muscle_force_avg < tuning18) { led(0,0,0,0,0,0) ; pwm_strike=tuning24; }
if (sum_muscle_force_avg > tuning18) { led(1,0,0,0,0,0) ; pwm_strike=tuning25; }
if (sum_muscle_force_avg > tuning19) { led(1,1,0,0,0,0) ; pwm_strike=tuning26; }
if (sum_muscle_force_avg > tuning20) { led(1,1,1,0,0,0) ; pwm_strike=tuning27; }
if (sum_muscle_force_avg > tuning21) { led(1,1,1,1,0,0) ; pwm_strike=tuning28; }
if (sum_muscle_force_avg > tuning22) { led(1,1,1,1,1,0) ; pwm_strike=tuning29; }
if (sum_muscle_force_avg > tuning23) { led(1,1,1,1,1,1) ; pwm_strike=tuning30; }
}
if(tuning31 == 1){f++;}
smp++;
}
if(smp>512)
{
pwm_motor_strike=pwm_strike;
if(tuning31==1){f=1;}
}
}
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 = 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++;
}
// ___________________________
// // \\
// || [ FILTER ] ||
// \\___________________________//
//
void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
{
EMG_left_Bicep = input1; EMG_Right_Bicep = input2; // Current input EMG left and right
EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep); // Highpassfilter
EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered); // Rectify
EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered); // Notch Filter part 1
EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1); // Notch Filter part 2
EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2); // Lowpassfilter
}
// ___________________________
// // \\
// || [TAKE SAMPLE] ||
// \\___________________________//
//
void take_sample() // Take a sample every 25th sample for moving average, every 5th sample ....
{
if(n==25)
{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)
{
//double Ku = (input1.read())*0.1; // EMG Right bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
//double Pu = (input2.read())*0.0001; // EMG Left bicep (tussen nul en 100%)
//pc.printf("Ku=%f Pu=%f \n\r", Ku, Pu);
P_gain_turn=tuning4; // 0.02569 // Change P,I,D values (activate)
I_gain_turn=tuning5;
D_gain_turn=tuning6; //Pu; 0.00014135;
}
void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
{
//double Ku = (input1.read())*0.01; // EMG Right bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
//double Pu = (input2.read())*0.00001; // EMG Left bicep (tussen nul en 100%)
P_gain_strike=tuning1;
//pc.printf("Ku=%f Pu=%f \n\r", Ku, Pu);
I_gain_strike=tuning2;
D_gain_strike=tuning3;
}
// ___________________________
// // \\
// || [OTHER FUNCTIONS] ||
// \\___________________________//
//
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");
}
void ControlGo() // Control flag
{ control_go = true; }
void setlooptimerflag(void) // Looptimerflag function
{ looptimerflag = true; }
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
{
// TEMPORARY USAGE WHILE POTMETER ACTIVE
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);
}
// Keep in range function
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;
}
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 led12() { ledgreen1 = ledon; ledgreen2 = ledon; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
void led23() { ledgreen1 = ledoff; ledgreen2 = ledon; ledyellow1 = ledon; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
void led34() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledon; ledyellow2 = ledon; ledred1 = ledoff; ledred2 = ledoff; }
void led45() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledon; ledred1 = ledon; ledred2 = ledoff; }
void led56() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledon; ledred2 = ledon; }
void led61() { ledgreen1 = ledon; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledon; }
void led_off() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
void led1() { ledgreen1 = ledon; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
void led2() { ledgreen1 = ledoff; ledgreen2 = ledon; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
void led3() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledon; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
void led4() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledon; ledred1 = ledoff; ledred2 = ledoff; }
void led5() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledon; ledred2 = ledoff; }
void led6() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledon; }
void led_on() { ledgreen1 = ledon; ledgreen2 = ledon; ledyellow1 = ledon; ledyellow2 = ledon; ledred1 = ledon; ledred2 = ledon; }
void leds_down_up()
{
if(a==15) {led1();}
if(a==30) {led2();}
if(a==45) {led3();}
if(a==60) {led4();}
if(a==90) {led5();}
if(a==90) {led6(); a=0;}
a++;
}
void led_up_down_up()
{
if(a==15) {led1();}
if(a==30) {led2();}
if(a==45) {led3();}
if(a==60) {led4();}
if(a==75) {led5();}
if(a==90) {led6();}
if(a==105) {led5();}
if(a==120) {led4();}
if(a==135) {led3();}
if(a==150) {led2();}
if(a==165) {led1(); a=0;}
a++;
}
