Wouter Bos / Mbed 2 deprecated 005-Motor_pot_PID-control

control flow doet nog niks

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@8:00c3992af9f4, 2015-10-14 (annotated)

Committer:
Zeekat
Date:
Wed Oct 14 13:19:32 2015 +0000
Revision:
8:00c3992af9f4
Parent:
7:4df9f6ebe744 
werkende versie. PID wordt getuned

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zeekat 0:f6306e179750 1 #include "mbed.h"
Zeekat 0:f6306e179750 2 #include "MODSERIAL.h"
Zeekat 0:f6306e179750 3 #include "encoder.h"
Zeekat 2:2563df4ee829 4 #include "HIDScope.h"
Zeekat 0:f6306e179750 5
Zeekat 0:f6306e179750 6 Serial pc(USBTX,USBRX);
Zeekat 8:00c3992af9f4 7 HIDScope scope(3); // definieerd het aantal kanalen van de scope
Zeekat 0:f6306e179750 8
Zeekat 2:2563df4ee829 9 // Define Tickers and control frequencies
Zeekat 0:f6306e179750 10 Ticker controller1, controller2; // definieer de ticker die controler1 doet
Zeekat 2:2563df4ee829 11 // Go flag variables
Zeekat 2:2563df4ee829 12 volatile bool motor1_go = false, motor2_go = false;
Zeekat 2:2563df4ee829 13
Zeekat 2:2563df4ee829 14 // Frequency control
Zeekat 2:2563df4ee829 15 double controlfreq = 50 ; // controlloops frequentie (Hz)
Zeekat 2:2563df4ee829 16 double controlstep = 1/controlfreq; // timestep derived from controlfreq
Zeekat 0:f6306e179750 17
Zeekat 2:2563df4ee829 18
Zeekat 2:2563df4ee829 19 //MOTOR OUTPUTPINS
Zeekat 0:f6306e179750 20 // motor 1
Zeekat 2:2563df4ee829 21 PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets)
Zeekat 2:2563df4ee829 22 DigitalOut motor1_rich(D7); // digitaal signaal voor richting
Zeekat 0:f6306e179750 23 // motor 2
Zeekat 2:2563df4ee829 24 PwmOut motor2_aan(D5);
Zeekat 2:2563df4ee829 25 DigitalOut motor2_rich(D4);
Zeekat 0:f6306e179750 26
Zeekat 2:2563df4ee829 27 // ENCODER INPUTPINS
Zeekat 0:f6306e179750 28 Encoder motor1_enc(D12,D11); // encoder outputpins
Zeekat 0:f6306e179750 29 Encoder motor2_enc(D10,D9);
Zeekat 0:f6306e179750 30
Zeekat 0:f6306e179750 31 int reference1 = 0; // set the reference position of the encoders
Zeekat 0:f6306e179750 32 int reference2 = 0;
Zeekat 0:f6306e179750 33
Zeekat 2:2563df4ee829 34
Zeekat 2:2563df4ee829 35 // EXTRA INPUTS AND REQUIRED VARIABLES
Zeekat 2:2563df4ee829 36 //POTMETERS
Zeekat 2:2563df4ee829 37 AnalogIn potright(A0); // define the potmeter outputpins
Zeekat 2:2563df4ee829 38 AnalogIn potleft(A1);
Zeekat 2:2563df4ee829 39
Zeekat 6:9c8e91bb1316 40 // BUTTONS
Zeekat 6:9c8e91bb1316 41 // control flow
Zeekat 4:072b99947fc6 42 DigitalIn buttonlinks(PTA4);
Zeekat 4:072b99947fc6 43 DigitalIn buttonrechts(PTC6);
Zeekat 4:072b99947fc6 44 // init values
Zeekat 4:072b99947fc6 45 bool loop_start = false;
Zeekat 4:072b99947fc6 46 bool calib_start = false;
Zeekat 6:9c8e91bb1316 47
Zeekat 6:9c8e91bb1316 48 // direction control
Zeekat 6:9c8e91bb1316 49 DigitalIn reverse_button_links(D0);
Zeekat 6:9c8e91bb1316 50 DigitalIn reverse_button_rechts(D1);
Zeekat 6:9c8e91bb1316 51 // init values
Zeekat 6:9c8e91bb1316 52 bool reverse_links = false;
Zeekat 6:9c8e91bb1316 53 bool reverse_rechts = false;
Zeekat 2:2563df4ee829 54
Zeekat 4:072b99947fc6 55 // LED
Zeekat 4:072b99947fc6 56 DigitalOut ledred(LED1);
Zeekat 4:072b99947fc6 57 DigitalOut ledgreen(LED2);
Zeekat 4:072b99947fc6 58 DigitalOut ledblue(LED3);
Zeekat 2:2563df4ee829 59
Zeekat 3:7273bbe6aa02 60 // REFERENCE SIGNAL SETTINGS
Zeekat 7:4df9f6ebe744 61 double input_threshold = 0.25; // the minimum value the signal must have to change the reference.
Zeekat 3:7273bbe6aa02 62 // Define signal amplification (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!) ??
Zeekat 3:7273bbe6aa02 63 double signalamp1 = 1;
Zeekat 3:7273bbe6aa02 64 double signalamp2 = 1;
Zeekat 6:9c8e91bb1316 65 // Define storage variables for reference values
Zeekat 3:7273bbe6aa02 66 double c_reference1 = 0;
Zeekat 3:7273bbe6aa02 67 double c_reference2 = 0;
Zeekat 8:00c3992af9f4 68 // limit angles
Zeekat 8:00c3992af9f4 69 // motor1
Zeekat 8:00c3992af9f4 70 const double limlow1 = 0.5; // min height
Zeekat 8:00c3992af9f4 71 const double limhigh1 = 4.5; // max height
Zeekat 8:00c3992af9f4 72 // motor 2
Zeekat 8:00c3992af9f4 73 const double limlow2 = -4.5; // maximum height, motor has been inversed due to transmission
Zeekat 8:00c3992af9f4 74 const double limhigh2 = 2; // minimum height
Zeekat 8:00c3992af9f4 75
Zeekat 3:7273bbe6aa02 76 // Define the maximum rate of change for the reference (velocity)
Zeekat 8:00c3992af9f4 77 double Vmax = 3; // rad/sec
Zeekat 2:2563df4ee829 78
Zeekat 0:f6306e179750 79 // CONTROLLER SETTINGS
Zeekat 1:2f23368e8638 80 // motor 1
Zeekat 8:00c3992af9f4 81 const double m1_Kp = 2; // Proportional constant
Zeekat 8:00c3992af9f4 82 const double m1_Ki = 0.05; // integration constant
Zeekat 1:2f23368e8638 83 const double m1_Kd = 0; // differentiation constant
Zeekat 1:2f23368e8638 84 // motor 2
Zeekat 8:00c3992af9f4 85 const double m2_Kp = 0.25;
Zeekat 8:00c3992af9f4 86 const double m2_Ki = 0.01;
Zeekat 1:2f23368e8638 87 const double m2_Kd = 0;
Zeekat 1:2f23368e8638 88 // storage variables
Zeekat 1:2f23368e8638 89 // motor 1
Zeekat 1:2f23368e8638 90 double m1_err_int = 0;
Zeekat 1:2f23368e8638 91 double m1_prev_err = 0;
Zeekat 1:2f23368e8638 92 // motor 2
Zeekat 1:2f23368e8638 93 double m2_err_int = 0;
Zeekat 1:2f23368e8638 94 double m2_prev_err = 0;
Zeekat 0:f6306e179750 95
Zeekat 0:f6306e179750 96
Zeekat 1:2f23368e8638 97 //// FILTER VARIABLES
Zeekat 1:2f23368e8638 98 // storage variables
Zeekat 1:2f23368e8638 99 // differential action filter, same is used for both controllers
Zeekat 1:2f23368e8638 100 double m_f_v1 = 0;
Zeekat 1:2f23368e8638 101 double m_f_v2 = 0;
Zeekat 1:2f23368e8638 102
Zeekat 1:2f23368e8638 103 // Filter coefficients
Zeekat 6:9c8e91bb1316 104 // differential action filter
Zeekat 8:00c3992af9f4 105 const double m_f_a1 = -1.1430, m_f_a2 = 0.4128, m_f_b0 = 0.0675, m_f_b1 = 0.1349, m_f_b2 = 0.0675; // coefficients from sheets are used as first test.
Zeekat 5:67afe491a1e3 106
Zeekat 5:67afe491a1e3 107 // tweede orde notch filter 50 Hz
Zeekat 5:67afe491a1e3 108 // biquad 1 coefficienten
Zeekat 5:67afe491a1e3 109 const double numnotch50biq1_1 = 1;
Zeekat 5:67afe491a1e3 110 const double numnotch50biq1_2 = -1.61816178466632;
Zeekat 5:67afe491a1e3 111 const double numnotch50biq1_3 = 1.00000006127058;
Zeekat 5:67afe491a1e3 112 const double dennotch50biq1_2 = -1.59325742941798;
Zeekat 5:67afe491a1e3 113 const double dennotch50biq1_3 = 0.982171881701431;
Zeekat 5:67afe491a1e3 114 // biquad 2 coefficienten
Zeekat 5:67afe491a1e3 115 const double numnotch50biq2_1 = 1;
Zeekat 5:67afe491a1e3 116 const double numnotch50biq2_2 = -1.61816171933244;
Zeekat 5:67afe491a1e3 117 const double numnotch50biq2_3 = 0.999999938729428;
Zeekat 5:67afe491a1e3 118 const double dennotch50biq2_2 = -1.61431180968071;
Zeekat 5:67afe491a1e3 119 const double dennotch50biq2_3 = 0.982599066293075;
Zeekat 5:67afe491a1e3 120 // highpass filter 20 Hz coefficienten
Zeekat 5:67afe491a1e3 121 const double numhigh20_1 = 0.837089190566345;
Zeekat 5:67afe491a1e3 122 const double numhigh20_2 = -1.67417838113269;
Zeekat 5:67afe491a1e3 123 const double numhigh20_3 = 0.837089190566345;
Zeekat 5:67afe491a1e3 124 const double denhigh20_2 = -1.64745998107698;
Zeekat 5:67afe491a1e3 125 const double denhigh20_3 = 0.700896781188403;
Zeekat 5:67afe491a1e3 126 // lowpass 5 Hz coefficienten
Zeekat 5:67afe491a1e3 127 const double numlow5_1 =0.000944691843840162;
Zeekat 5:67afe491a1e3 128 const double numlow5_2 =0.00188938368768032;
Zeekat 5:67afe491a1e3 129 const double numlow5_3 =0.000944691843840162;
Zeekat 5:67afe491a1e3 130 const double denlow5_2 =-1.91119706742607;
Zeekat 5:67afe491a1e3 131 const double denlow5_3 =0.914975834801434;
Zeekat 5:67afe491a1e3 132
Zeekat 5:67afe491a1e3 133 // Define the storage variables and filter coeicients for two filters
Zeekat 5:67afe491a1e3 134 double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0;
Zeekat 0:f6306e179750 135
Zeekat 0:f6306e179750 136
Zeekat 0:f6306e179750 137
Zeekat 0:f6306e179750 138 ////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 139 /////////////////// START OF SIDE FUNCTIONS ////////////////////
Zeekat 0:f6306e179750 140 //////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 141 // these functions are tailored to perform 1 specific function
Zeekat 0:f6306e179750 142
Zeekat 5:67afe491a1e3 143 // this funtion flips leds on and off accordin to input with 0 being on
Zeekat 5:67afe491a1e3 144 void LED(int red,int green,int blue)
Zeekat 5:67afe491a1e3 145 {
Zeekat 5:67afe491a1e3 146 ledred.write(red);
Zeekat 5:67afe491a1e3 147 ledgreen.write(green);
Zeekat 5:67afe491a1e3 148 ledblue.write(blue);
Zeekat 5:67afe491a1e3 149 }
Zeekat 5:67afe491a1e3 150
Zeekat 0:f6306e179750 151 // counts 2 radians
Zeekat 0:f6306e179750 152 // this function takes counts from the encoder and converts it to the amount of radians from the zero position.
Zeekat 0:f6306e179750 153 // It has been set up for standard 2X DECODING!!!!!!
Zeekat 0:f6306e179750 154 double get_radians(double counts)
Zeekat 0:f6306e179750 155 {
Zeekat 0:f6306e179750 156 double pi = 3.14159265359;
Zeekat 0:f6306e179750 157 double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, extra warning)
Zeekat 0:f6306e179750 158 return radians;
Zeekat 0:f6306e179750 159 }
Zeekat 0:f6306e179750 160
Zeekat 0:f6306e179750 161
Zeekat 7:4df9f6ebe744 162 // This functions takes a 0->1 input, uses passing by reference (&c_reference)
Zeekat 7:4df9f6ebe744 163 // to create a reference that moves with a variable speed. It is now optimised for 0->1 values
Zeekat 8:00c3992af9f4 164 double reference_f(double input, double &c_reference, double limlow, double limhigh)
Zeekat 0:f6306e179750 165 {
Zeekat 6:9c8e91bb1316 166 double reference = c_reference + input * controlstep * Vmax ;
Zeekat 8:00c3992af9f4 167 if(reference < limlow){reference = limlow;}
Zeekat 8:00c3992af9f4 168 if(reference > limhigh){reference = limhigh;}
Zeekat 3:7273bbe6aa02 169 c_reference = reference; // change the global variable to the latest location.
Zeekat 3:7273bbe6aa02 170 return reference;
Zeekat 0:f6306e179750 171 }
Zeekat 0:f6306e179750 172
Zeekat 0:f6306e179750 173
Zeekat 3:7273bbe6aa02 174 // This function takes the controller outputvalue and ensures it is between -1 and 1
Zeekat 0:f6306e179750 175 // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
Zeekat 3:7273bbe6aa02 176 // needs more work to use it for the wind-up prevention.
Zeekat 2:2563df4ee829 177 double outputlimiter (double output, double limit)
Zeekat 0:f6306e179750 178 {
Zeekat 2:2563df4ee829 179 if(output> limit)
Zeekat 0:f6306e179750 180 {
Zeekat 0:f6306e179750 181 output = 1;
Zeekat 0:f6306e179750 182 }
Zeekat 2:2563df4ee829 183 else if(output < limit && output > 0)
Zeekat 0:f6306e179750 184 {
Zeekat 0:f6306e179750 185 output = output;
Zeekat 0:f6306e179750 186 }
Zeekat 2:2563df4ee829 187 else if(output > -limit && output < 0)
Zeekat 0:f6306e179750 188 {
Zeekat 0:f6306e179750 189 output = output;
Zeekat 0:f6306e179750 190 }
Zeekat 2:2563df4ee829 191 else if(output < -limit)
Zeekat 0:f6306e179750 192 {
Zeekat 0:f6306e179750 193 (output = -1);
Zeekat 0:f6306e179750 194 }
Zeekat 0:f6306e179750 195 return output;
Zeekat 0:f6306e179750 196 }
Zeekat 0:f6306e179750 197
Zeekat 0:f6306e179750 198
Zeekat 1:2f23368e8638 199 // BIQUADFILTER CODE GIVEN IN SHEETS
Zeekat 1:2f23368e8638 200 // used for d-control
Zeekat 1:2f23368e8638 201 double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
Zeekat 1:2f23368e8638 202 {
Zeekat 3:7273bbe6aa02 203 double v = u - a1*v1 - a2*v2;
Zeekat 3:7273bbe6aa02 204 double y = b0*v + b1*v1 + b2*v2;
Zeekat 1:2f23368e8638 205 v2 = v1;
Zeekat 1:2f23368e8638 206 v1 = v;
Zeekat 1:2f23368e8638 207 return y;
Zeekat 1:2f23368e8638 208 }
Zeekat 1:2f23368e8638 209
Zeekat 5:67afe491a1e3 210 double EMG_Filter(double u1)
Zeekat 5:67afe491a1e3 211 {
Zeekat 5:67afe491a1e3 212 // double u1 = potright.read();
Zeekat 5:67afe491a1e3 213 double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
Zeekat 5:67afe491a1e3 214 double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
Zeekat 5:67afe491a1e3 215 double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
Zeekat 5:67afe491a1e3 216 double y4 = abs(y3);
Zeekat 5:67afe491a1e3 217 double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
Zeekat 5:67afe491a1e3 218 return y5;
Zeekat 5:67afe491a1e3 219 }
Zeekat 1:2f23368e8638 220
Zeekat 1:2f23368e8638 221 // PID Controller given in sheets
Zeekat 1:2f23368e8638 222 // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!)
Zeekat 2:2563df4ee829 223 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
Zeekat 1:2f23368e8638 224 {
Zeekat 1:2f23368e8638 225 // Proportional
Zeekat 1:2f23368e8638 226 double P = Kp * e;
Zeekat 8:00c3992af9f4 227 // pc.printf("P = %f, ",P);
Zeekat 1:2f23368e8638 228 // Integral
Zeekat 1:2f23368e8638 229 e_int = e_int + Ts * e;
Zeekat 1:2f23368e8638 230 double I = e_int * Ki;
Zeekat 8:00c3992af9f4 231 // pc.printf("I = %f, ",I);
Zeekat 3:7273bbe6aa02 232 // Derivative (Disabled for now because of NaN problem from filter
Zeekat 3:7273bbe6aa02 233 double e_derr = (e - e_prev)/Ts;
Zeekat 8:00c3992af9f4 234 // pc.printf("e_derr %f, ",e_derr);
Zeekat 3:7273bbe6aa02 235 e_derr = biquadfilter(e_derr, m_f_v1, m_f_v2, m_f_a1, m_f_a2, m_f_b0, m_f_b1, m_f_b2);
Zeekat 8:00c3992af9f4 236 //
Zeekat 3:7273bbe6aa02 237 pc.printf("fil_e_derr %f, ",e_derr); // check for NaN ??
Zeekat 3:7273bbe6aa02 238 e_prev = e;
Zeekat 8:00c3992af9f4 239 double D = Kd* e_derr;
Zeekat 1:2f23368e8638 240
Zeekat 1:2f23368e8638 241 // PID
Zeekat 8:00c3992af9f4 242 double output = P + I + D;
Zeekat 1:2f23368e8638 243 return output;
Zeekat 1:2f23368e8638 244 }
Zeekat 1:2f23368e8638 245
Zeekat 1:2f23368e8638 246
Zeekat 1:2f23368e8638 247
Zeekat 0:f6306e179750 248 // this function is a simple K control called by the motor function
Zeekat 0:f6306e179750 249 double K_control(double error,double K)
Zeekat 0:f6306e179750 250 {
Zeekat 0:f6306e179750 251 double output = K*error; // controller output K*e
Zeekat 2:2563df4ee829 252 output = outputlimiter(output,1); // limit the output to -1->1
Zeekat 0:f6306e179750 253 return output;
Zeekat 0:f6306e179750 254 }
Zeekat 0:f6306e179750 255
Zeekat 0:f6306e179750 256 /////////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 257 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
Zeekat 0:f6306e179750 258 ///////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 259 // these functions are used to control all aspects of a single electric motor and are called by the main function
Zeekat 0:f6306e179750 260
Zeekat 0:f6306e179750 261 // MOTOR 1
Zeekat 0:f6306e179750 262 void motor1_control()
Zeekat 0:f6306e179750 263 {
Zeekat 5:67afe491a1e3 264
Zeekat 7:4df9f6ebe744 265 double input1 = potright.read()*signalamp1; // this line must be seperated for emg usage
Zeekat 8:00c3992af9f4 266 //pc.printf("input = %f ",input1);
Zeekat 7:4df9f6ebe744 267 // first data limiter
Zeekat 7:4df9f6ebe744 268 if(input1 < input_threshold) {input1 = 0;}
Zeekat 6:9c8e91bb1316 269 if(input1 > 1) {input1 = 1;}
Zeekat 7:4df9f6ebe744 270 // reverse direction
Zeekat 6:9c8e91bb1316 271 if(reverse_rechts == true) {input1 = -input1;}
Zeekat 8:00c3992af9f4 272
Zeekat 8:00c3992af9f4 273 double reference1 = reference_f(input1, c_reference1,limlow1,limhigh1); // determine the reference that has been set by the inputsignal
Zeekat 8:00c3992af9f4 274 scope.set(0,reference1);
Zeekat 0:f6306e179750 275 double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 8:00c3992af9f4 276 scope.set(1,rads1);
Zeekat 3:7273bbe6aa02 277 double error1 = (reference1 - rads1); // determine the error (reference - position)
Zeekat 1:2f23368e8638 278 double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
Zeekat 8:00c3992af9f4 279 //pc.printf("output 1 %f \n",output1);
Zeekat 8:00c3992af9f4 280 scope.set(2,output1);
Zeekat 8:00c3992af9f4 281 scope.send();
Zeekat 8:00c3992af9f4 282 output1 = outputlimiter(output1,1);
Zeekat 0:f6306e179750 283 if(output1 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:f6306e179750 284 motor1_rich.write(0);
Zeekat 0:f6306e179750 285 motor1_aan.write(output1);
Zeekat 0:f6306e179750 286 } else if(output1 < 0) {
Zeekat 0:f6306e179750 287 motor1_rich.write(1);
Zeekat 0:f6306e179750 288 motor1_aan.write(abs(output1));
Zeekat 0:f6306e179750 289 }
Zeekat 0:f6306e179750 290 }
Zeekat 0:f6306e179750 291
Zeekat 0:f6306e179750 292 // MOTOR 2
Zeekat 0:f6306e179750 293 void motor2_control()
Zeekat 0:f6306e179750 294 {
Zeekat 5:67afe491a1e3 295 double input2 = potleft.read()*signalamp2; // replace potleft with filter
Zeekat 7:4df9f6ebe744 296 // first input limiter
Zeekat 7:4df9f6ebe744 297 if(input2 < input_threshold) {input2 = 0;}
Zeekat 6:9c8e91bb1316 298 if(input2 > 1) {input2 = 1;}
Zeekat 7:4df9f6ebe744 299 // reverse direction
Zeekat 6:9c8e91bb1316 300 if(reverse_links == true) {input2 = -input2;}
Zeekat 8:00c3992af9f4 301 double reference2 = reference_f(input2, c_reference2,limlow2,limhigh2); // determine the reference that has been set by the clamped inputsignal
Zeekat 0:f6306e179750 302 double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
Zeekat 3:7273bbe6aa02 303 double error2 = (reference2 - rads2); // determine the error (reference - position)
Zeekat 8:00c3992af9f4 304 //scope.set(1,error2);
Zeekat 8:00c3992af9f4 305 //scope.send();
Zeekat 1:2f23368e8638 306 double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
Zeekat 8:00c3992af9f4 307 output2 = outputlimiter(output2,1);
Zeekat 0:f6306e179750 308 if(output2 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:f6306e179750 309 motor2_rich.write(0);
Zeekat 0:f6306e179750 310 motor2_aan.write(output2);
Zeekat 0:f6306e179750 311 } else if(output2 < 0) {
Zeekat 0:f6306e179750 312 motor2_rich.write(1);
Zeekat 0:f6306e179750 313 motor2_aan.write(abs(output2));
Zeekat 0:f6306e179750 314 }
Zeekat 0:f6306e179750 315 }
Zeekat 0:f6306e179750 316
Zeekat 0:f6306e179750 317
Zeekat 2:2563df4ee829 318 //////////////////////////////////////////////////////////////////
Zeekat 2:2563df4ee829 319 //////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
Zeekat 2:2563df4ee829 320 ////////////////////////////////////////////////////////////////
Zeekat 2:2563df4ee829 321
Zeekat 2:2563df4ee829 322 void motor1_activate()
Zeekat 2:2563df4ee829 323 {
Zeekat 2:2563df4ee829 324 motor1_go = true;
Zeekat 2:2563df4ee829 325 }
Zeekat 2:2563df4ee829 326
Zeekat 2:2563df4ee829 327 void motor2_activate()
Zeekat 2:2563df4ee829 328 {
Zeekat 2:2563df4ee829 329 motor2_go = true;
Zeekat 2:2563df4ee829 330 }
Zeekat 2:2563df4ee829 331
Zeekat 0:f6306e179750 332 int main()
Zeekat 0:f6306e179750 333 {
Zeekat 2:2563df4ee829 334 pc.baud(115200);
Zeekat 4:072b99947fc6 335 controller1.attach(&motor1_activate, controlstep); // call a go-flag
Zeekat 8:00c3992af9f4 336 controller2.attach(&motor2_activate, controlstep); // Disabled while debugging PID-controller ??
Zeekat 0:f6306e179750 337 while(true)
Zeekat 0:f6306e179750 338 {
Zeekat 6:9c8e91bb1316 339 // button press functions
Zeekat 6:9c8e91bb1316 340 // flow buttons
Zeekat 4:072b99947fc6 341 if(buttonlinks.read() == 0)
Zeekat 2:2563df4ee829 342 {
Zeekat 6:9c8e91bb1316 343 loop_start = !loop_start;
Zeekat 4:072b99947fc6 344 wait(buttonlinks.read() == 1);
Zeekat 6:9c8e91bb1316 345 wait(0.3);
Zeekat 2:2563df4ee829 346 }
Zeekat 4:072b99947fc6 347 if(buttonrechts.read() == 0)
Zeekat 4:072b99947fc6 348 {
Zeekat 6:9c8e91bb1316 349 calib_start = !calib_start;
Zeekat 4:072b99947fc6 350 wait(buttonrechts.read() == 1);
Zeekat 6:9c8e91bb1316 351 wait(0.3);
Zeekat 4:072b99947fc6 352 }
Zeekat 6:9c8e91bb1316 353 // reverse buttons
Zeekat 6:9c8e91bb1316 354 if(reverse_button_links.read() == 0)
Zeekat 6:9c8e91bb1316 355 {
Zeekat 7:4df9f6ebe744 356 reverse_links = !reverse_links;
Zeekat 7:4df9f6ebe744 357 wait(reverse_button_links.read() == 1);
Zeekat 7:4df9f6ebe744 358 wait(0.3);
Zeekat 6:9c8e91bb1316 359 }
Zeekat 7:4df9f6ebe744 360 if(reverse_button_rechts.read() == 0)
Zeekat 6:9c8e91bb1316 361 {
Zeekat 6:9c8e91bb1316 362 reverse_rechts = !reverse_rechts;
Zeekat 6:9c8e91bb1316 363 wait(reverse_button_rechts.read() == 1);
Zeekat 6:9c8e91bb1316 364 wait(0.3);
Zeekat 6:9c8e91bb1316 365 }
Zeekat 4:072b99947fc6 366 // Main Control stuff and options
Zeekat 4:072b99947fc6 367 if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops
Zeekat 2:2563df4ee829 368 {
Zeekat 5:67afe491a1e3 369 LED(1,1,0); // turn blue led on
Zeekat 7:4df9f6ebe744 370 if(motor1_go) { motor1_go = false; motor1_control();}
Zeekat 7:4df9f6ebe744 371 if(motor2_go) { motor2_go = false; motor2_control();}
Zeekat 2:2563df4ee829 372 }
Zeekat 7:4df9f6ebe744 373 // turn green led on // start calibration procedures
Zeekat 8:00c3992af9f4 374 if(loop_start == false && calib_start == true) { LED(1,0,1); motor1_aan.write(0); motor2_aan.write(0);}
Zeekat 7:4df9f6ebe744 375 // turn red led on
Zeekat 7:4df9f6ebe744 376 if(loop_start == true && calib_start == true) { LED(0,1,1);}
Zeekat 7:4df9f6ebe744 377 // turn leds off (both buttons false)
Zeekat 7:4df9f6ebe744 378 else { LED(1,1,1);}
Zeekat 0:f6306e179750 379 }
Zeekat 0:f6306e179750 380 }