Thomas Burgers / Mbed 2 deprecated 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@10:09ba965045a7, 2015-10-07 (annotated)

Committer:
ThomasBNL
Date:
Wed Oct 07 12:53:13 2015 +0000
Revision:
10:09ba965045a7
Parent:
9:697134d3564e
Child:
11:ecd83b303252
werkende PI controller, gain I uitzoeken

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThomasBNL 0:40052f5ca77b 1 #include "mbed.h"
ThomasBNL 0:40052f5ca77b 2 //#include "HIDScope.h"
ThomasBNL 0:40052f5ca77b 3 #include "QEI.h"
ThomasBNL 0:40052f5ca77b 4 #include "MODSERIAL.h"
ThomasBNL 8:50d6e2323d3b 5 #include "biquadFilter.h"
ThomasBNL 0:40052f5ca77b 6 #include "encoder.h"
ThomasBNL 0:40052f5ca77b 7
ThomasBNL 7:ddd7fb357786 8 MODSERIAL pc(USBTX,USBRX);
ThomasBNL 8:50d6e2323d3b 9
ThomasBNL 8:50d6e2323d3b 10 // (DEBUGGING AND TESTING BUTTONS) (0 when pressed and 1 when not pressed)
ThomasBNL 8:50d6e2323d3b 11 DigitalIn buttonL1(PTC6); // Button 1 (laag op board) for testing at the lower board
ThomasBNL 8:50d6e2323d3b 12 DigitalIn buttonL2(PTA4); // Button 2 (laag op board) for testing at the lower board
ThomasBNL 8:50d6e2323d3b 13 DigitalIn buttonH1(D2); // Button 3 (hoog op board) for testing at the top board
ThomasBNL 8:50d6e2323d3b 14 DigitalIn buttonH2(D6); // Button 4 (hoog op board) for testing at the top board
ThomasBNL 0:40052f5ca77b 15
ThomasBNL 0:40052f5ca77b 16 volatile bool looptimerflag;
ThomasBNL 8:50d6e2323d3b 17 const double cw=0; // zero is clockwise (front view)
ThomasBNL 8:50d6e2323d3b 18 const double ccw=1; // one is counterclockwise (front view)
ThomasBNL 8:50d6e2323d3b 19
ThomasBNL 8:50d6e2323d3b 20 const double Gain_P_turn=0.0067;
ThomasBNL 8:50d6e2323d3b 21 // stel setpoint tussen (0 en 360) en position tussen (0 en 360)
ThomasBNL 8:50d6e2323d3b 22 // 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
ThomasBNL 8:50d6e2323d3b 23 // dus 0.1=15*gain gain=0.0067
ThomasBNL 8:50d6e2323d3b 24
ThomasBNL 8:50d6e2323d3b 25 double conversion_counts_to_degrees=0.085877862594198;
ThomasBNL 8:50d6e2323d3b 26 // gear ratio motor = 131
ThomasBNL 8:50d6e2323d3b 27 // ticks per magnet rotation = 32 (X2 Encoder)
ThomasBNL 8:50d6e2323d3b 28 // One revolution = 360 degrees
ThomasBNL 8:50d6e2323d3b 29 // degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
ThomasBNL 8:50d6e2323d3b 30
ThomasBNL 8:50d6e2323d3b 31 const double sample_time=0.01; // tijd voor een sample (100Hz)
ThomasBNL 8:50d6e2323d3b 32
ThomasBNL 8:50d6e2323d3b 33 // PID motor constants
ThomasBNL 8:50d6e2323d3b 34 double integrate_error_turn=0; // integration error turn motor
ThomasBNL 8:50d6e2323d3b 35 double previous_error_turn=0; // previous error turn motor
ThomasBNL 8:50d6e2323d3b 36
ThomasBNL 1:dc683e88b44e 37
ThomasBNL 7:ddd7fb357786 38 // Functions used (described after main)
ThomasBNL 7:ddd7fb357786 39 void keep_in_range(double * in, double min, double max);
ThomasBNL 7:ddd7fb357786 40 void setlooptimerflag(void);
ThomasBNL 8:50d6e2323d3b 41 double get_reference(double input);
ThomasBNL 8:50d6e2323d3b 42
ThomasBNL 0:40052f5ca77b 43
ThomasBNL 7:ddd7fb357786 44 // MAIN function
ThomasBNL 0:40052f5ca77b 45 int main() {
ThomasBNL 8:50d6e2323d3b 46 AnalogIn potmeter(A0); // Potmeter that can read a reference value (DEBUG TOOL)
ThomasBNL 8:50d6e2323d3b 47 QEI motor_turn(D12,D13,NC,32); // Encoder for motor Turn
ThomasBNL 8:50d6e2323d3b 48 PwmOut pwm_motor_turn(D5); // Pwm for motor Turn
ThomasBNL 8:50d6e2323d3b 49 DigitalOut motordirection_turn(D4); // Direction of the motor Turn
ThomasBNL 8:50d6e2323d3b 50 double reference_turn; // Set constant to store reference value of the Turn motor
ThomasBNL 8:50d6e2323d3b 51 double position_turn; // Set constant to store current position of the Turn motor
ThomasBNL 8:50d6e2323d3b 52 double error;
ThomasBNL 10:09ba965045a7 53 double pwm_to_motor_turn;
ThomasBNL 10:09ba965045a7 54 double pwm_motor_turn_P;
ThomasBNL 10:09ba965045a7 55 double pwm_motor_turn_I;
ThomasBNL 10:09ba965045a7 56 double pwm_motor_turn_D;
ThomasBNL 0:40052f5ca77b 57
ThomasBNL 7:ddd7fb357786 58 //START OF CODE
ThomasBNL 7:ddd7fb357786 59 pc.printf("Start of code \n\r");
ThomasBNL 0:40052f5ca77b 60
ThomasBNL 8:50d6e2323d3b 61 pc.baud(9600); // Set the baudrate
ThomasBNL 0:40052f5ca77b 62
ThomasBNL 7:ddd7fb357786 63 // Tickers
ThomasBNL 8:50d6e2323d3b 64 Ticker looptimer; // Ticker called looptimer to set a looptimerflag
ThomasBNL 8:50d6e2323d3b 65 looptimer.attach(setlooptimerflag,sample_time); // calls the looptimer flag every 0.01s
ThomasBNL 0:40052f5ca77b 66
ThomasBNL 7:ddd7fb357786 67 pc.printf("Start infinite loop \n\r");
ThomasBNL 8:50d6e2323d3b 68 wait (3); // Rest before starting system
ThomasBNL 0:40052f5ca77b 69
ThomasBNL 0:40052f5ca77b 70 //INFINITE LOOP
ThomasBNL 5:8fb74a22fe3c 71 while(1)
ThomasBNL 8:50d6e2323d3b 72 { // Start while loop
ThomasBNL 8:50d6e2323d3b 73 // DEBUGGING BUTTON: interrupt button Disbalances the current motor position
ThomasBNL 8:50d6e2323d3b 74 if (buttonL2.read() < 0.5){ //if button pressed
ThomasBNL 8:50d6e2323d3b 75 motordirection_turn = cw;
ThomasBNL 8:50d6e2323d3b 76 pwm_motor_turn = 0.5f; // motorspeed
ThomasBNL 8:50d6e2323d3b 77 pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
ThomasBNL 8:50d6e2323d3b 78
ThomasBNL 8:50d6e2323d3b 79 // Wait until looptimer flag is true then execute PID controller.
ThomasBNL 5:8fb74a22fe3c 80 else
ThomasBNL 8:50d6e2323d3b 81 {
ThomasBNL 0:40052f5ca77b 82 while(looptimerflag != true);
ThomasBNL 0:40052f5ca77b 83
ThomasBNL 0:40052f5ca77b 84 looptimerflag = false;
ThomasBNL 0:40052f5ca77b 85
ThomasBNL 8:50d6e2323d3b 86 //reference = (potmeter.read()-0.5)*2000; // Potmeter bepaald reference (uitgeschakeld)
ThomasBNL 8:50d6e2323d3b 87 reference_turn = 15;
ThomasBNL 0:40052f5ca77b 88
ThomasBNL 8:50d6e2323d3b 89 // Keep motor position between -4200 and 4200 counts
ThomasBNL 8:50d6e2323d3b 90 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
ThomasBNL 3:11a7da46e093 91 {
ThomasBNL 8:50d6e2323d3b 92 motor_turn.reset();
ThomasBNL 3:11a7da46e093 93 pc.printf("RESET \n\r");
ThomasBNL 3:11a7da46e093 94 }
ThomasBNL 3:11a7da46e093 95
ThomasBNL 8:50d6e2323d3b 96 // Convert position to degrees
ThomasBNL 8:50d6e2323d3b 97 position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
ThomasBNL 0:40052f5ca77b 98
ThomasBNL 8:50d6e2323d3b 99 pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
ThomasBNL 3:11a7da46e093 100
ThomasBNL 0:40052f5ca77b 101
ThomasBNL 8:50d6e2323d3b 102 // P-CONTROLLER
ThomasBNL 8:50d6e2323d3b 103 // Calculate error then multiply it with the gain, and store in pwm_to_motor
ThomasBNL 8:50d6e2323d3b 104
ThomasBNL 10:09ba965045a7 105 error=(reference_turn - position_turn); // Current error (input controller)
ThomasBNL 8:50d6e2323d3b 106
ThomasBNL 10:09ba965045a7 107 integrate_error_turn=integrate_error_turn + sample_time*error; // integral error output
ThomasBNL 8:50d6e2323d3b 108 // // overwrite previous integrate error by adding the current error multiplied by the sample time.
ThomasBNL 8:50d6e2323d3b 109 //
ThomasBNL 8:50d6e2323d3b 110 //double error_derivative_turn=(error - previous_error_turn)/sample_time; // derivative error output
ThomasBNL 8:50d6e2323d3b 111
ThomasBNL 8:50d6e2323d3b 112 // FILTER error_derivative_turn (lowpassfilter)
ThomasBNL 8:50d6e2323d3b 113 // biquadFilter Lowpassfilter(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
ThomasBNL 10:09ba965045a7 114 //const double mT_f_a1=-1.965293372622690e+00;
ThomasBNL 8:50d6e2323d3b 115 //const double mT_f_a2=9.658854605688177e-01;
ThomasBNL 8:50d6e2323d3b 116 //const double mT_f_b0=1.480219865318266e-04;
ThomasBNL 8:50d6e2323d3b 117 //const double mT_f_b1=2.960439730636533e-04;
ThomasBNL 8:50d6e2323d3b 118 //const double mT_f_b2=1.480219865318266e-04; // Motor Turn filter constants
ThomasBNL 8:50d6e2323d3b 119 // Lowpassfilter.step(e_der)
ThomasBNL 3:11a7da46e093 120
ThomasBNL 9:697134d3564e 121 //previous_error_turn=error; // current error is saved to memory constant to be used in
ThomasBNL 8:50d6e2323d3b 122 // the next loop for calculating the derivative error
ThomasBNL 8:50d6e2323d3b 123
ThomasBNL 10:09ba965045a7 124 double Gain_I_turn=0.01;
ThomasBNL 9:697134d3564e 125 // double Gain_D_turn=1;
ThomasBNL 8:50d6e2323d3b 126
ThomasBNL 10:09ba965045a7 127 pwm_to_motor_turn = error*Gain_P_turn; // output P controller to pwm
ThomasBNL 8:50d6e2323d3b 128
ThomasBNL 10:09ba965045a7 129 pwm_motor_turn_P = error*Gain_P_turn; // output P controller to pwm
ThomasBNL 10:09ba965045a7 130 pwm_motor_turn_I = integrate_error_turn*Gain_I_turn; // output I controller to pwm
ThomasBNL 8:50d6e2323d3b 131 // double pwm_motor_turn_D = error_derivative_turn*Gain_D_turn; // output D controller to pwm
ThomasBNL 10:09ba965045a7 132
ThomasBNL 10:09ba965045a7 133 pwm_to_motor_turn = pwm_motor_turn_P + integrate_error_turn*Gain_I_turn;
ThomasBNL 10:09ba965045a7 134
ThomasBNL 8:50d6e2323d3b 135 //
ThomasBNL 10:09ba965045a7 136 // double pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D; // Total output PID controller to pwm
ThomasBNL 8:50d6e2323d3b 137 //
ThomasBNL 0:40052f5ca77b 138
ThomasBNL 8:50d6e2323d3b 139 // Keep Pwm between -1 and 1
ThomasBNL 10:09ba965045a7 140 keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 10:09ba965045a7 141 pc.printf("pwm %f \n\r", pwm_to_motor_turn);
ThomasBNL 0:40052f5ca77b 142
ThomasBNL 8:50d6e2323d3b 143 // Check error and decide direction to turn
ThomasBNL 10:09ba965045a7 144 if(pwm_to_motor_turn > 0)
ThomasBNL 3:11a7da46e093 145 {
ThomasBNL 8:50d6e2323d3b 146 motordirection_turn=ccw;
ThomasBNL 3:11a7da46e093 147 pc.printf("if loop pwm_to_motor > 0 \n\r");
ThomasBNL 3:11a7da46e093 148 }
ThomasBNL 0:40052f5ca77b 149 else
ThomasBNL 3:11a7da46e093 150 {
ThomasBNL 8:50d6e2323d3b 151 motordirection_turn=cw;
ThomasBNL 3:11a7da46e093 152 pc.printf("else loop pwm_to_motor < 0 \n\r");
ThomasBNL 3:11a7da46e093 153 }
ThomasBNL 8:50d6e2323d3b 154
ThomasBNL 8:50d6e2323d3b 155 // Put pwm_motor to the motor
ThomasBNL 10:09ba965045a7 156 pwm_motor_turn=(abs(pwm_to_motor_turn));
ThomasBNL 0:40052f5ca77b 157 }
ThomasBNL 0:40052f5ca77b 158 }
ThomasBNL 5:8fb74a22fe3c 159 }
ThomasBNL 0:40052f5ca77b 160
ThomasBNL 0:40052f5ca77b 161 // Keep in range function
ThomasBNL 0:40052f5ca77b 162 void keep_in_range(double * in, double min, double max)
ThomasBNL 0:40052f5ca77b 163 {
ThomasBNL 0:40052f5ca77b 164 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 0:40052f5ca77b 165 }
ThomasBNL 0:40052f5ca77b 166
ThomasBNL 0:40052f5ca77b 167 // Looptimerflag function
ThomasBNL 0:40052f5ca77b 168 void setlooptimerflag(void)
ThomasBNL 0:40052f5ca77b 169 {
ThomasBNL 0:40052f5ca77b 170 looptimerflag = true;
ThomasBNL 1:dc683e88b44e 171 }
ThomasBNL 1:dc683e88b44e 172
ThomasBNL 8:50d6e2323d3b 173 // Get setpoint -> potmeter (MOMENTEEL UITGESCHAKELD)
ThomasBNL 8:50d6e2323d3b 174 double get_reference(double input)
ThomasBNL 1:dc683e88b44e 175 {
ThomasBNL 1:dc683e88b44e 176 const float offset = 0.5;
ThomasBNL 1:dc683e88b44e 177 const float gain = 4.0;
ThomasBNL 1:dc683e88b44e 178 return (input-offset)*gain;
ThomasBNL 5:8fb74a22fe3c 179 }