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:
- 8:50d6e2323d3b
- Parent:
- 7:ddd7fb357786
- Child:
- 9:697134d3564e
--- a/main.cpp Tue Oct 06 11:59:45 2015 +0000
+++ b/main.cpp Wed Oct 07 12:23:40 2015 +0000
@@ -2,105 +2,151 @@
//#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
-//#include "biquadFilter.h"
+#include "biquadFilter.h"
#include "encoder.h"
MODSERIAL pc(USBTX,USBRX);
-DigitalIn button(PTA4); // PTA4 is used as a button controller (0 when pressed and 1 when not pressed)
+
+// (DEBUGGING AND TESTING BUTTONS) (0 when pressed and 1 when not pressed)
+DigitalIn buttonL1(PTC6); // Button 1 (laag op board) for testing at the lower board
+DigitalIn buttonL2(PTA4); // Button 2 (laag op board) for testing at the lower board
+DigitalIn buttonH1(D2); // Button 3 (hoog op board) for testing at the top board
+DigitalIn buttonH2(D6); // Button 4 (hoog op board) for testing at the top board
volatile bool looptimerflag;
-const double cw=0; // zero is clockwise (front view)
-const double ccw=1; // one is counterclockwise (front view)
+const double cw=0; // zero is clockwise (front view)
+const double ccw=1; // one is counterclockwise (front view)
+
+const double Gain_P_turn=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
+
+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 sample_time=0.01; // tijd voor een sample (100Hz)
+
+// PID motor constants
+double integrate_error_turn=0; // integration error turn motor
+double previous_error_turn=0; // previous error turn motor
+
// Functions used (described after main)
void keep_in_range(double * in, double min, double max);
void setlooptimerflag(void);
-double get_degrees_from_counts(int counts);
-double get_setpoint(double input);
+double get_reference(double input);
+
// MAIN function
int main() {
- AnalogIn potmeter(A0);
- QEI motor1(D12,D13,NC,32);
- PwmOut pwm_motor(D5);
- DigitalOut motordir(D4);
- double setpoint;
- double pwm_to_motor;
- double position;
+ AnalogIn potmeter(A0); // Potmeter that can read a reference value (DEBUG TOOL)
+ 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
+ double reference_turn; // Set constant to store reference value of the Turn motor
+ double position_turn; // Set constant to store current position of the Turn motor
+ double error;
//START OF CODE
pc.printf("Start of code \n\r");
- pc.baud(9600); // Set the baudrate
+ pc.baud(9600); // Set the baudrate
// Tickers
- Ticker looptimer; // Ticker called looptimer to set a looptimerflag
- looptimer.attach(setlooptimerflag,0.01); // calls the looptimer flag every 0.01s
+ Ticker looptimer; // Ticker called looptimer to set a looptimerflag
+ looptimer.attach(setlooptimerflag,sample_time); // calls the looptimer flag every 0.01s
pc.printf("Start infinite loop \n\r");
- wait (3);
+ wait (3); // Rest before starting system
//INFINITE LOOP
-
while(1)
- { // Start while loop
- if (button.read() < 0.5){ //if button pressed
- motordir = cw; // zero is clockwise (front view)
- pwm_motor = 0.5f; // motorspeed
-
- pc.printf("positie = %d \r\n", motor1.getPulses()); }
+ { // Start while loop
+ // DEBUGGING BUTTON: interrupt button Disbalances the current motor position
+ if (buttonL2.read() < 0.5){ //if button pressed
+ motordirection_turn = cw;
+ pwm_motor_turn = 0.5f; // motorspeed
+ pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
+
+ // Wait until looptimer flag is true then execute PID controller.
else
- {
- /* Wait until looptimer flag is true. */
+ {
while(looptimerflag != true);
looptimerflag = false;
- // Setpoint calibration
- //setpoint = (potmeter.read()-0.5)*2000;
- setpoint = 15;
+ //reference = (potmeter.read()-0.5)*2000; // Potmeter bepaald reference (uitgeschakeld)
+ reference_turn = 15;
- // Position calibration
- if ((motor1.getPulses()>4200) || (motor1.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
+ // Keep motor position between -4200 and 4200 counts
+ if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
{
- motor1.reset();
+ motor_turn.reset();
pc.printf("RESET \n\r");
}
-
-// 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 conversion_to_degree_counts=0.085877862594198;
- position = conversion_to_degree_counts * motor1.getPulses();
+ // Convert position to degrees
+ position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
- pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", setpoint, motor1.getPulses(), position);
+ pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
- // This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor
- // 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
+ // P-CONTROLLER
+ // Calculate error then multiply it with the gain, and store in pwm_to_motor
+
+ error=(reference_turn - position_turn); // Current error (input controller)
+
+ // integrate_error_turn=integrate_error_turn + sample_time*error; // 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
+
+ // FILTER error_derivative_turn (lowpassfilter)
+ // biquadFilter Lowpassfilter(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
+ // 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
+ // Lowpassfilter.step(e_der)
- pwm_to_motor = (setpoint - position)*0.0067 + e_prev;
- e_prev=(setpoint - position)
+ previous_error_turn=error; // current error is saved to memory constant to be used in
+ // the next loop for calculating the derivative error
+
+ double Gain_I_turn=1;
+ double Gain_D_turn=1;
+
+ double pwm_motor_turn = error*Gain_P_turn; // output P controller to pwm
+
+ // double pwm_motor_turn_P = error*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
+//
+// double pwm_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D; // Total output PID controller to pwm
+//
- keep_in_range(&pwm_to_motor, -1,1); // Pass to motor controller but keep it in range!
- pc.printf("pwm %f \n\r", pwm_to_motor);
+ // Keep Pwm between -1 and 1
+ keep_in_range(&pwm_motor_turn, -1,1); // Pass to motor controller but keep it in range!
+ pc.printf("pwm %f \n\r", pwm_motor_turn);
- if(pwm_to_motor > 0)
+ // Check error and decide direction to turn
+ if(pwm_motor_turn > 0)
{
- motordir=ccw;
+ motordirection_turn=ccw;
pc.printf("if loop pwm_to_motor > 0 \n\r");
}
else
{
- motordir=cw;
+ motordirection_turn=cw;
pc.printf("else loop pwm_to_motor < 0 \n\r");
}
- pwm_motor=(abs(pwm_to_motor));
+
+ // Put pwm_motor to the motor
+ pwm_motor_turn=(abs(pwm_motor_turn));
}
}
}
@@ -117,8 +163,8 @@
looptimerflag = true;
}
-// Get setpoint -> potmeter
-double get_setpoint(double input)
+// Get setpoint -> potmeter (MOMENTEEL UITGESCHAKELD)
+double get_reference(double input)
{
const float offset = 0.5;
const float gain = 4.0;
