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
Diff: main.cpp
- Revision:
- 43:fb69ef657f30
- Parent:
- 42:0a7898cb3e94
- Child:
- 44:5dd0a3d24662
--- a/main.cpp Wed Oct 14 11:39:13 2015 +0000
+++ b/main.cpp Wed Oct 14 13:14:14 2015 +0000
@@ -1,3 +1,20 @@
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// ___________________________
+// // \\
+// || [Libraries] ||
+// \\___________________________//
+//
+
#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
@@ -18,12 +35,12 @@
DigitalOut debug_led_green(LED_GREEN); // Debug LED
DigitalOut debug_led_blue(LED_BLUE); // Debug LED
-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
+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
-AnalogIn potmeter(A0); // Potmeter that can read a reference value (DEBUG TOOL)
+AnalogIn potmeter(A0); // Potmeter that can read a reference value (DEBUG TOOL)
// ___________________________
// // \\
@@ -31,9 +48,27 @@
// \\___________________________//
//
-QEI motor_turn(D12,D13,NC,32); // Encoder for motor Turn
-PwmOut pwm_motor_turn(D5); // Pwm for motor Turn
-DigitalOut motordirection_turn(D4); // Direction of the motor Turn
+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; // integration error turn motor
+double previous_error_turn=0; // previous error turn motor
+
+// ___________________________
+// // \\
+// || [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
// ___________________________
// // \\
@@ -41,45 +76,26 @@
// \\___________________________//
//
-HIDScope scope(2); // HIDSCOPE declared
+HIDScope scope(3); // HIDSCOPE declared
// ___________________________
// // \\
-// || [CONSTANTS] ||
+// || [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; // sampling frequency (512 Hz)
-
-// PID motor constants
-double integrate_error_turn=0; // integration error turn motor
-double previous_error_turn=0; // previous error turn motor
-
-double P_gain_turn=10; //0.0067;
- // stel setpoint tussen (0 en 360) en position tussen (0 en 360)
- // max verschil: 360 -> dan pwm_to_motor 1 tot aan een verschil van 15 graden-> bij 15 moet pwm_to_motor ong 0.1 zijn
- // dus 0.1=15*gain gain=0.0067
- // Als 3 graden verschil 0.1 dan 0.1/3=gain=0.33
-
-double I_gain_turn=0.1; //0.025; //(1/2000) //0.00000134
- // pwm_motor_I=(integrate_error_turn + sample_time*error)*gain; pwm = (4*0.01 + 4)* Gain => 0.1 pwm gewenst (na 1 seconde een verschil van 4 graden)
- // 0.1 / (4.01) = Gain = 0.025
-
-double D_gain_turn=50; //0.01;
- // error_derivative_turn=(error - previous_error_turn)/sample_time
- //
-
-double conversion_counts_to_degrees=0.085877862594198;
- // 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 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
// ___________________________
// // \\
@@ -87,43 +103,39 @@
// \\___________________________//
//
-const double mT_f_a1=-1.965293372622690e+00;
+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; // Motor Turn filter constants
+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 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
// ___________________________
// // \\
// || [FUNCTIONS USED] ||
// \\___________________________//
//
+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 keep_in_range(double * in, double min, double max);
-void setlooptimerflag(void);
-double get_reference(double input);
+void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain); // STRIKE/TURN: Deactivate PID Controller
-// STRIKE: Deactivate PID Controller
-void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain);
-// TURN: Activate PID Controller
-void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain);
-// STRIKE: Activate PID Controller
-void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain);
+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
-// Change reference position (two functions)
-void Change_Turn_Position_Left (double& reference, double change_one_bottle);
-
-void Change_Turn_Position_Right (double& reference, double change_one_bottle);
///////////////////////////////
// //
/////////////////////////////////////////////////////// [MAIN FUNCTION] ////////////////////////////////////////////////////////////////////////////
// // //
/////////////////////////////// //
+
int main() {
+
debug_led_red=off;
debug_led_blue=off;
debug_led_green=off;
@@ -135,40 +147,65 @@
double pwm_motor_turn_P;
double pwm_motor_turn_I;
double pwm_motor_turn_D;
- double reference_turn=0; // Set constant to store reference value of the Turn motor
+// ___________________________
+// // \\
+// || [START OF CODE] ||
+// \\___________________________//
+// START OF CODE
+ pc.printf("Start of code \n\r");
+
+ pc.baud(115200); // Set the baudrate
- // CALIBRATE (STARTING POSITION)
+// ___________________________
+// // \\
+// || [CALIBRATE] ||
+// \\___________________________//
+// Calibrate starting postion (RED LED)
+
+ 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 ((buttonH2.read() < 0.5) && (buttonH2.read() < 0.5)) // Save current TURN and STRIKE positions as starting position
+ {
+ motor_turn.reset(); // TURN: Starting Position
+ double 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_complete; // Calibration complete
+ }
+ }
+
+ calibration_complete:
+ debug_led_red=off;
- // TEST: Does change PID gains work? => works correctly
- deactivate_PID_Controller(P_gain_turn,I_gain_turn,D_gain_turn); // Deactivate Turn motor
- pc.printf("P = %d I = %d D = %d \n\r");
- activate_PID_Controller_turn(P_gain_turn,I_gain_turn,D_gain_turn);
- pc.printf("P = %d I = %d D = %d \n\r");
-
- // TEST: Does change reference function work? => still needs to be tested
- pc.printf("initial reference_turn = %d \n\r", reference_turn);
- Change_Turn_Position_Left (reference_turn, change_one_bottle);
- pc.printf("After left turn reference_turn = %d \n\r", reference_turn);
- Change_Turn_Position_Right (reference_turn, change_one_bottle);
- pc.printf("After right turn reference_turn = %d \n\r", reference_turn);
- Change_Turn_Position_Right (reference_turn, change_one_bottle);
- Change_Turn_Position_Right (reference_turn, change_one_bottle);
- Change_Turn_Position_Right (reference_turn, change_one_bottle);
- Change_Turn_Position_Right (reference_turn, change_one_bottle);
- pc.printf("After right turn reference_turn = %d \n\r", reference_turn); // DOES THE LIMITER WORK? (should be -90 degrees)
-
- //START OF CODE
- pc.printf("Start of code \n\r");
-
- pc.baud(115200); // Set the baudrate
+
// Tickers
Ticker looptimer; // Ticker called looptimer to set a looptimerflag
looptimer.attach(setlooptimerflag,(float)1/Fs); // calls the looptimer flag every 0.01s
pc.printf("Start infinite loop \n\r");
- wait (3); // Wait before starting system
+ wait (5); // Wait before starting system
//INFINITE LOOP
@@ -326,14 +363,6 @@
looptimerflag = true;
}
-// Get setpoint -> potmeter (MOMENTEEL UITGESCHAKELD)
-double get_reference(double input)
-{
-const float offset = 0.5;
-const float gain = 4.0;
-return (input-offset)*gain;
-}
-
// Change reference
void Change_Turn_Position_Left (double& reference, double change_one_bottle)
{
