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:
- 37:23660d12d772
- Parent:
- 36:da07b5c2984d
--- a/main.cpp Tue Oct 13 21:37:23 2015 +0000
+++ b/main.cpp Wed Oct 14 05:35:11 2015 +0000
@@ -30,7 +30,6 @@
// || [MOTOR TURN] ||
// \\___________________________//
//
-
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
@@ -40,22 +39,86 @@
// || [HIDSCOPE] ||
// \\___________________________//
//
-
-HIDScope scope(2); // HIDSCOPE declared
-
+//HIDScope scope(3); // HIDSCOPE declared
// ___________________________
// // \\
// || [CONSTANTS] ||
// \\___________________________//
//
+ const double off=1; //Led off
+ const double on=0; //Led on
+ const int Fs = 512; // sampling frequency (512 Hz)
+
+// ___________________________
+// // \\
+// || [FUNCTIONS USED] ||
+// \\___________________________//
+//
-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)
+void keep_in_range(double * in, double min, double max);
+void setlooptimerflag(void);
+
+
+
+ ///////////////////////////////
+ // //
+/////////////////////////////////////////////////////// [MAIN FUNCTION] ////////////////////////////////////////////////////////////////////////////
+ // // //
+ /////////////////////////////// //
+int main() {
+ debug_led_red=off;
+ debug_led_blue=off;
+ debug_led_green=off;
+
+
+ //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
+
+
+ //INFINITE LOOP
+ while(1)
+ { // Start while loop
+
+
+// ___________________________
+// // \\
+// || [Wait for go signal] ||
+// \\___________________________//
+// // Wait until looptimer flag is true then execute PID controller loop.
+
+
+ //reference = (potmeter.read()-0.5)*2000; // Potmeter bepaald reference (uitgeschakeld)
+
+
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // //
+ // [Functions Described] //
+ // //
+ ////////////////////////////////////
+
+// Keep in range function
+void keep_in_range(double * in, double min, double max)
+{
+ *in > min ? *in < max? : *in = max: *in = min;
+}
+
+ double reference_turn=20; // Set constant to store reference value of the Turn motor
+ volatile bool looptimerflag;
+ const double cw=0; // zero is clockwise (front view)
+ const double ccw=1; // one is counterclockwise (front view)
+ const double sample_time = 0.001953125; // sampling frequency (512 Hz)
// PID motor constants
double integrate_error_turn=0; // integration error turn motor
@@ -74,61 +137,19 @@
double Gain_D_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
-
-// ___________________________
-// // \\
-// || [FUNCTIONS USED] ||
-// \\___________________________//
-//
-
-void keep_in_range(double * in, double min, double max);
-void setlooptimerflag(void);
-double get_reference(double input);
-
+// Looptimerflag function
+void setlooptimerflag(void)
+{
+ debug_led_green=on;
- ///////////////////////////////
- // //
-/////////////////////////////////////////////////////// [MAIN FUNCTION] ////////////////////////////////////////////////////////////////////////////
- // // //
- /////////////////////////////// //
-int main() {
- debug_led_red=off;
- debug_led_blue=off;
- debug_led_green=off;
-
- double position_turn; // Set constant to store current position of the Turn motor
- double error;
- double pwm_to_motor_turn;
- double pwm_motor_turn_P;
- double pwm_motor_turn_I;
- double pwm_motor_turn_D;
- double reference_turn=0; // Set constant to store reference value of the Turn motor
-
- //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
-
-
- //INFINITE LOOP
- while(1)
- { // Start while loop
-
// ___________________________
// // \\
// || [DEBUG BUTTONS] ||
@@ -151,24 +172,13 @@
else
{
-// ___________________________
-// // \\
-// || [Wait for go signal] ||
-// \\___________________________//
-// // Wait until looptimer flag is true then execute PID controller loop.
-
- while(looptimerflag != true);
-
- looptimerflag = false;
-
- //reference = (potmeter.read()-0.5)*2000; // Potmeter bepaald reference (uitgeschakeld)
-
-// ___________________________
+
+ // ___________________________
// // \\
// || [Calibrate position] ||
// \\___________________________//
// // Keep motor position between -4200 and 4200 counts
-
+ debug_led_green=on;
if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
{
motor_turn.reset();
@@ -177,7 +187,7 @@
// Convert position to degrees \\
- position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
+ double position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
@@ -188,34 +198,33 @@
// \\___________________________//
// // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\
- error=(reference_turn - position_turn); // Current error (input controller)
+ double error_turn=(reference_turn - position_turn); // Current error (input controller)
- integrate_error_turn=integrate_error_turn + sample_time*error; // integral error output
+ integrate_error_turn=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_turn=(error - previous_error_turn)/sample_time; // derivative error output
+ double error_derivative_turn=(error_turn - previous_error_turn)/sample_time; // derivative error output
- // FILTER error_derivative_turn (lowpassfilter)
+ // FILTER error_derivative_turn (lowpassfilter) (EMG LOWPASS FILTER MOMENTEEL!!!!!)
- const double mT_f_a1=-1.965293372622690e+00;
- 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
-
- biquadFilter Lowpassfilter(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
+const double low_b1 = 1.480219865318266e-04; //filter coefficients
+const double low_b2 = 2.960439730636533e-04;
+const double low_b3 = 1.480219865318266e-04;
+const double low_a2 = -1.965293372622690e+00; // a1 is normalized to 1
+const double low_a3 = 9.658854605688177e-01;
+biquadFilter lowpassfilter_1(low_a2, low_a3, low_b1, low_b2, low_b3);
- error_derivative_turn=Lowpassfilter.step(error_derivative_turn);
+ error_derivative_turn=lowpassfilter_1.step(error_derivative_turn);
- previous_error_turn=error; // current error is saved to memory constant to be used in
+ previous_error_turn=error_turn; // current error is saved to memory constant to be used in
// the next loop for calculating the derivative error
- pwm_to_motor_turn = error*Gain_P_turn; // output P controller to pwm
+ double pwm_to_motor_turn = error_turn*Gain_P_turn; // output P controller to pwm
- pwm_motor_turn_P = error*Gain_P_turn; // output P controller to pwm
- pwm_motor_turn_I = integrate_error_turn*Gain_I_turn; // output I controller to pwm
- pwm_motor_turn_D = error_derivative_turn*Gain_D_turn; // output D controller to pwm
+ double pwm_motor_turn_P = error_turn*Gain_P_turn; // output P controller to pwm
+ double pwm_motor_turn_I = integrate_error_turn*Gain_I_turn; // output I controller to pwm
+ double pwm_motor_turn_D = error_derivative_turn*Gain_D_turn; // output D controller to pwm
pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D;
@@ -254,30 +263,11 @@
// \\___________________________//
// // Check signals inside HIDSCOPE \\
- scope.set(0,error); // HIDSCOPE channel 0 : Current Reference
- scope.set(1,position_turn); // HIDSCOPE channel 1 : Position_turn
- scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn
- scope.send(); // Send channel info to HIDSCOPE
- }
-}
+// scope.set(0,error_turn); // HIDSCOPE channel 0 : Current Reference
+// scope.set(1,position_turn); // HIDSCOPE channel 1 : Position_turn
+// scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn
+// scope.send(); // Send channel info to HIDSCOPE
}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // //
- // [Functions Described] //
- // //
- ////////////////////////////////////
-
-// Keep in range function
-void keep_in_range(double * in, double min, double max)
-{
- *in > min ? *in < max? : *in = max: *in = min;
-}
-
-// Looptimerflag function
-void setlooptimerflag(void)
-{
- looptimerflag = true;
}
// Get setpoint -> potmeter (MOMENTEEL UITGESCHAKELD)
