Wouter Bos / Mbed 2 deprecated 005-Motor_pot_PID-control

control flow doet nog niks

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@6:9c8e91bb1316, 2015-10-10 (annotated)

Committer:
Zeekat
Date:
Sat Oct 10 20:38:38 2015 +0000
Revision:
6:9c8e91bb1316
Parent:
5:67afe491a1e3
Child:
7:4df9f6ebe744
added extra buttons to reverse motion, and removed the mirrored pot input. now pot can be directly replaced with filtered input without changing anything (i hope). the gain determination has not been implemented yet.

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 2:2563df4ee829 7 HIDScope scope(2); // 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 3:7273bbe6aa02 61 // Define signal amplification (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!) ??
Zeekat 3:7273bbe6aa02 62 double signalamp1 = 1;
Zeekat 3:7273bbe6aa02 63 double signalamp2 = 1;
Zeekat 6:9c8e91bb1316 64 // Define storage variables for reference values
Zeekat 3:7273bbe6aa02 65 double c_reference1 = 0;
Zeekat 3:7273bbe6aa02 66 double c_reference2 = 0;
Zeekat 3:7273bbe6aa02 67 // Define the maximum rate of change for the reference (velocity)
Zeekat 2:2563df4ee829 68 double Vmax = 5; // rad/sec
Zeekat 2:2563df4ee829 69
Zeekat 0:f6306e179750 70 // CONTROLLER SETTINGS
Zeekat 1:2f23368e8638 71 // motor 1
Zeekat 1:2f23368e8638 72 const double m1_Kp = 1; // Proportional constant
Zeekat 2:2563df4ee829 73 const double m1_Ki = 1; // integration constant
Zeekat 1:2f23368e8638 74 const double m1_Kd = 0; // differentiation constant
Zeekat 1:2f23368e8638 75 // motor 2
Zeekat 1:2f23368e8638 76 const double m2_Kp = 1;
Zeekat 1:2f23368e8638 77 const double m2_Ki = 0;
Zeekat 1:2f23368e8638 78 const double m2_Kd = 0;
Zeekat 1:2f23368e8638 79 // storage variables
Zeekat 1:2f23368e8638 80 // motor 1
Zeekat 1:2f23368e8638 81 double m1_err_int = 0;
Zeekat 1:2f23368e8638 82 double m1_prev_err = 0;
Zeekat 1:2f23368e8638 83 // motor 2
Zeekat 1:2f23368e8638 84 double m2_err_int = 0;
Zeekat 1:2f23368e8638 85 double m2_prev_err = 0;
Zeekat 0:f6306e179750 86
Zeekat 0:f6306e179750 87
Zeekat 1:2f23368e8638 88 //// FILTER VARIABLES
Zeekat 1:2f23368e8638 89 // storage variables
Zeekat 1:2f23368e8638 90 // differential action filter, same is used for both controllers
Zeekat 1:2f23368e8638 91 double m_f_v1 = 0;
Zeekat 1:2f23368e8638 92 double m_f_v2 = 0;
Zeekat 1:2f23368e8638 93
Zeekat 1:2f23368e8638 94 // Filter coefficients
Zeekat 6:9c8e91bb1316 95 // differential action filter
Zeekat 6:9c8e91bb1316 96 const double m_f_a1 = 1.0, m_f_a2 = 2.0, m_f_b0 = 1.0, m_f_b1 = 3.0, m_f_b2 = 4.0; // coefficients from sheets are used as first test.
Zeekat 5:67afe491a1e3 97
Zeekat 5:67afe491a1e3 98 // tweede orde notch filter 50 Hz
Zeekat 5:67afe491a1e3 99 // biquad 1 coefficienten
Zeekat 5:67afe491a1e3 100 const double numnotch50biq1_1 = 1;
Zeekat 5:67afe491a1e3 101 const double numnotch50biq1_2 = -1.61816178466632;
Zeekat 5:67afe491a1e3 102 const double numnotch50biq1_3 = 1.00000006127058;
Zeekat 5:67afe491a1e3 103 const double dennotch50biq1_2 = -1.59325742941798;
Zeekat 5:67afe491a1e3 104 const double dennotch50biq1_3 = 0.982171881701431;
Zeekat 5:67afe491a1e3 105 // biquad 2 coefficienten
Zeekat 5:67afe491a1e3 106 const double numnotch50biq2_1 = 1;
Zeekat 5:67afe491a1e3 107 const double numnotch50biq2_2 = -1.61816171933244;
Zeekat 5:67afe491a1e3 108 const double numnotch50biq2_3 = 0.999999938729428;
Zeekat 5:67afe491a1e3 109 const double dennotch50biq2_2 = -1.61431180968071;
Zeekat 5:67afe491a1e3 110 const double dennotch50biq2_3 = 0.982599066293075;
Zeekat 5:67afe491a1e3 111 // highpass filter 20 Hz coefficienten
Zeekat 5:67afe491a1e3 112 const double numhigh20_1 = 0.837089190566345;
Zeekat 5:67afe491a1e3 113 const double numhigh20_2 = -1.67417838113269;
Zeekat 5:67afe491a1e3 114 const double numhigh20_3 = 0.837089190566345;
Zeekat 5:67afe491a1e3 115 const double denhigh20_2 = -1.64745998107698;
Zeekat 5:67afe491a1e3 116 const double denhigh20_3 = 0.700896781188403;
Zeekat 5:67afe491a1e3 117 // lowpass 5 Hz coefficienten
Zeekat 5:67afe491a1e3 118 const double numlow5_1 =0.000944691843840162;
Zeekat 5:67afe491a1e3 119 const double numlow5_2 =0.00188938368768032;
Zeekat 5:67afe491a1e3 120 const double numlow5_3 =0.000944691843840162;
Zeekat 5:67afe491a1e3 121 const double denlow5_2 =-1.91119706742607;
Zeekat 5:67afe491a1e3 122 const double denlow5_3 =0.914975834801434;
Zeekat 5:67afe491a1e3 123
Zeekat 5:67afe491a1e3 124 // Define the storage variables and filter coeicients for two filters
Zeekat 5:67afe491a1e3 125 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 126
Zeekat 0:f6306e179750 127
Zeekat 0:f6306e179750 128
Zeekat 0:f6306e179750 129 ////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 130 /////////////////// START OF SIDE FUNCTIONS ////////////////////
Zeekat 0:f6306e179750 131 //////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 132 // these functions are tailored to perform 1 specific function
Zeekat 0:f6306e179750 133
Zeekat 5:67afe491a1e3 134 // this funtion flips leds on and off accordin to input with 0 being on
Zeekat 5:67afe491a1e3 135 void LED(int red,int green,int blue)
Zeekat 5:67afe491a1e3 136 {
Zeekat 5:67afe491a1e3 137 ledred.write(red);
Zeekat 5:67afe491a1e3 138 ledgreen.write(green);
Zeekat 5:67afe491a1e3 139 ledblue.write(blue);
Zeekat 5:67afe491a1e3 140 }
Zeekat 5:67afe491a1e3 141
Zeekat 0:f6306e179750 142 // counts 2 radians
Zeekat 0:f6306e179750 143 // this function takes counts from the encoder and converts it to the amount of radians from the zero position.
Zeekat 0:f6306e179750 144 // It has been set up for standard 2X DECODING!!!!!!
Zeekat 0:f6306e179750 145 double get_radians(double counts)
Zeekat 0:f6306e179750 146 {
Zeekat 0:f6306e179750 147 double pi = 3.14159265359;
Zeekat 0:f6306e179750 148 double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, extra warning)
Zeekat 0:f6306e179750 149 return radians;
Zeekat 0:f6306e179750 150 }
Zeekat 0:f6306e179750 151
Zeekat 0:f6306e179750 152
Zeekat 3:7273bbe6aa02 153 // This functions takes a 0->1 input converts it to -1->1 and uses passing by reference (&c_reference)
Zeekat 0:f6306e179750 154 // to create a reference that moves with a variable speed. It is now set up for potmeter values.
Zeekat 3:7273bbe6aa02 155 double reference_f(double input, double &c_reference)
Zeekat 0:f6306e179750 156 {
Zeekat 6:9c8e91bb1316 157 double reference = c_reference + input * controlstep * Vmax ;
Zeekat 3:7273bbe6aa02 158 c_reference = reference; // change the global variable to the latest location.
Zeekat 3:7273bbe6aa02 159 return reference;
Zeekat 0:f6306e179750 160 }
Zeekat 0:f6306e179750 161
Zeekat 0:f6306e179750 162
Zeekat 3:7273bbe6aa02 163 // This function takes the controller outputvalue and ensures it is between -1 and 1
Zeekat 0:f6306e179750 164 // 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 165 // needs more work to use it for the wind-up prevention.
Zeekat 2:2563df4ee829 166 double outputlimiter (double output, double limit)
Zeekat 0:f6306e179750 167 {
Zeekat 2:2563df4ee829 168 if(output> limit)
Zeekat 0:f6306e179750 169 {
Zeekat 0:f6306e179750 170 output = 1;
Zeekat 0:f6306e179750 171 }
Zeekat 2:2563df4ee829 172 else if(output < limit && output > 0)
Zeekat 0:f6306e179750 173 {
Zeekat 0:f6306e179750 174 output = output;
Zeekat 0:f6306e179750 175 }
Zeekat 2:2563df4ee829 176 else if(output > -limit && output < 0)
Zeekat 0:f6306e179750 177 {
Zeekat 0:f6306e179750 178 output = output;
Zeekat 0:f6306e179750 179 }
Zeekat 2:2563df4ee829 180 else if(output < -limit)
Zeekat 0:f6306e179750 181 {
Zeekat 0:f6306e179750 182 (output = -1);
Zeekat 0:f6306e179750 183 }
Zeekat 0:f6306e179750 184 return output;
Zeekat 0:f6306e179750 185 }
Zeekat 0:f6306e179750 186
Zeekat 0:f6306e179750 187
Zeekat 1:2f23368e8638 188 // BIQUADFILTER CODE GIVEN IN SHEETS
Zeekat 1:2f23368e8638 189 // used for d-control
Zeekat 1:2f23368e8638 190 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 191 {
Zeekat 3:7273bbe6aa02 192 double v = u - a1*v1 - a2*v2;
Zeekat 3:7273bbe6aa02 193 double y = b0*v + b1*v1 + b2*v2;
Zeekat 1:2f23368e8638 194 v2 = v1;
Zeekat 1:2f23368e8638 195 v1 = v;
Zeekat 1:2f23368e8638 196 return y;
Zeekat 1:2f23368e8638 197 }
Zeekat 1:2f23368e8638 198
Zeekat 5:67afe491a1e3 199 double EMG_Filter(double u1)
Zeekat 5:67afe491a1e3 200 {
Zeekat 5:67afe491a1e3 201 // double u1 = potright.read();
Zeekat 5:67afe491a1e3 202 double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
Zeekat 5:67afe491a1e3 203 double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
Zeekat 5:67afe491a1e3 204 double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
Zeekat 5:67afe491a1e3 205 double y4 = abs(y3);
Zeekat 5:67afe491a1e3 206 double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
Zeekat 5:67afe491a1e3 207 return y5;
Zeekat 5:67afe491a1e3 208 }
Zeekat 1:2f23368e8638 209
Zeekat 1:2f23368e8638 210 // PID Controller given in sheets
Zeekat 1:2f23368e8638 211 // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!)
Zeekat 2:2563df4ee829 212 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
Zeekat 1:2f23368e8638 213 {
Zeekat 1:2f23368e8638 214 // Proportional
Zeekat 1:2f23368e8638 215 double P = Kp * e;
Zeekat 3:7273bbe6aa02 216 pc.printf("P = %f, ",P);
Zeekat 1:2f23368e8638 217 // Integral
Zeekat 1:2f23368e8638 218 e_int = e_int + Ts * e;
Zeekat 1:2f23368e8638 219 double I = e_int * Ki;
Zeekat 3:7273bbe6aa02 220 pc.printf("I = %f, ",I);
Zeekat 3:7273bbe6aa02 221 // Derivative (Disabled for now because of NaN problem from filter
Zeekat 3:7273bbe6aa02 222 double e_derr = (e - e_prev)/Ts;
Zeekat 3:7273bbe6aa02 223 pc.printf("e_derr %f, ",e_derr);
Zeekat 3:7273bbe6aa02 224 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 3:7273bbe6aa02 225 pc.printf("fil_e_derr %f, ",e_derr); // check for NaN ??
Zeekat 3:7273bbe6aa02 226 e_prev = e;
Zeekat 3:7273bbe6aa02 227 // double D = Kd* e_derr;
Zeekat 1:2f23368e8638 228
Zeekat 1:2f23368e8638 229 // PID
Zeekat 2:2563df4ee829 230 double output = P + I ;// + D;
Zeekat 1:2f23368e8638 231 return output;
Zeekat 1:2f23368e8638 232 }
Zeekat 1:2f23368e8638 233
Zeekat 1:2f23368e8638 234
Zeekat 1:2f23368e8638 235
Zeekat 0:f6306e179750 236 // this function is a simple K control called by the motor function
Zeekat 0:f6306e179750 237 double K_control(double error,double K)
Zeekat 0:f6306e179750 238 {
Zeekat 0:f6306e179750 239 double output = K*error; // controller output K*e
Zeekat 2:2563df4ee829 240 output = outputlimiter(output,1); // limit the output to -1->1
Zeekat 0:f6306e179750 241 return output;
Zeekat 0:f6306e179750 242 }
Zeekat 0:f6306e179750 243
Zeekat 0:f6306e179750 244 /////////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 245 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
Zeekat 0:f6306e179750 246 ///////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 247 // these functions are used to control all aspects of a single electric motor and are called by the main function
Zeekat 0:f6306e179750 248
Zeekat 0:f6306e179750 249 // MOTOR 1
Zeekat 0:f6306e179750 250 void motor1_control()
Zeekat 0:f6306e179750 251 {
Zeekat 5:67afe491a1e3 252
Zeekat 3:7273bbe6aa02 253 double input1 = potright.read()*signalamp1;
Zeekat 6:9c8e91bb1316 254 if(input1 < 0.25) {input1 = 0;}
Zeekat 6:9c8e91bb1316 255 if(input1 > 1) {input1 = 1;}
Zeekat 6:9c8e91bb1316 256 if(reverse_rechts == true) {input1 = -input1;}
Zeekat 3:7273bbe6aa02 257 double reference1 = reference_f(input1, c_reference1); // determine the reference that has been set by the inputsignal
Zeekat 0:f6306e179750 258 double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 3:7273bbe6aa02 259 double error1 = (reference1 - rads1); // determine the error (reference - position)
Zeekat 1:2f23368e8638 260 double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
Zeekat 2:2563df4ee829 261 pc.printf("output 1 %f \n",output1);
Zeekat 0:f6306e179750 262 if(output1 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:f6306e179750 263 motor1_rich.write(0);
Zeekat 0:f6306e179750 264 motor1_aan.write(output1);
Zeekat 0:f6306e179750 265 } else if(output1 < 0) {
Zeekat 0:f6306e179750 266 motor1_rich.write(1);
Zeekat 0:f6306e179750 267 motor1_aan.write(abs(output1));
Zeekat 0:f6306e179750 268 }
Zeekat 0:f6306e179750 269 }
Zeekat 0:f6306e179750 270
Zeekat 0:f6306e179750 271 // MOTOR 2
Zeekat 0:f6306e179750 272 void motor2_control()
Zeekat 0:f6306e179750 273 {
Zeekat 5:67afe491a1e3 274 double input2 = potleft.read()*signalamp2; // replace potleft with filter
Zeekat 6:9c8e91bb1316 275 if(input2 < 0.25) {input2 = 0;}
Zeekat 6:9c8e91bb1316 276 if(input2 > 1) {input2 = 1;}
Zeekat 6:9c8e91bb1316 277 if(reverse_links == true) {input2 = -input2;}
Zeekat 6:9c8e91bb1316 278 double reference2 = reference_f(input2, c_reference2); // determine the reference that has been set by the clamped inputsignal
Zeekat 0:f6306e179750 279 double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
Zeekat 3:7273bbe6aa02 280 double error2 = (reference2 - rads2); // determine the error (reference - position)
Zeekat 1:2f23368e8638 281 double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
Zeekat 0:f6306e179750 282 if(output2 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:f6306e179750 283 motor2_rich.write(0);
Zeekat 0:f6306e179750 284 motor2_aan.write(output2);
Zeekat 0:f6306e179750 285 } else if(output2 < 0) {
Zeekat 0:f6306e179750 286 motor2_rich.write(1);
Zeekat 0:f6306e179750 287 motor2_aan.write(abs(output2));
Zeekat 0:f6306e179750 288 }
Zeekat 0:f6306e179750 289 }
Zeekat 0:f6306e179750 290
Zeekat 0:f6306e179750 291
Zeekat 2:2563df4ee829 292 //////////////////////////////////////////////////////////////////
Zeekat 2:2563df4ee829 293 //////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
Zeekat 2:2563df4ee829 294 ////////////////////////////////////////////////////////////////
Zeekat 2:2563df4ee829 295
Zeekat 2:2563df4ee829 296 void motor1_activate()
Zeekat 2:2563df4ee829 297 {
Zeekat 2:2563df4ee829 298 motor1_go = true;
Zeekat 2:2563df4ee829 299 }
Zeekat 2:2563df4ee829 300
Zeekat 2:2563df4ee829 301 void motor2_activate()
Zeekat 2:2563df4ee829 302 {
Zeekat 2:2563df4ee829 303 motor2_go = true;
Zeekat 2:2563df4ee829 304 }
Zeekat 2:2563df4ee829 305
Zeekat 0:f6306e179750 306 int main()
Zeekat 0:f6306e179750 307 {
Zeekat 2:2563df4ee829 308 pc.baud(115200);
Zeekat 4:072b99947fc6 309 controller1.attach(&motor1_activate, controlstep); // call a go-flag
Zeekat 4:072b99947fc6 310 // controller2.attach(&motor2_activate, controlstep); Disabled while debugging PID-controller ??
Zeekat 0:f6306e179750 311 while(true)
Zeekat 0:f6306e179750 312 {
Zeekat 6:9c8e91bb1316 313 // button press functions
Zeekat 6:9c8e91bb1316 314 // flow buttons
Zeekat 4:072b99947fc6 315 if(buttonlinks.read() == 0)
Zeekat 2:2563df4ee829 316 {
Zeekat 6:9c8e91bb1316 317 loop_start = !loop_start;
Zeekat 4:072b99947fc6 318 wait(buttonlinks.read() == 1);
Zeekat 6:9c8e91bb1316 319 wait(0.3);
Zeekat 2:2563df4ee829 320 }
Zeekat 4:072b99947fc6 321 if(buttonrechts.read() == 0)
Zeekat 4:072b99947fc6 322 {
Zeekat 6:9c8e91bb1316 323 calib_start = !calib_start;
Zeekat 4:072b99947fc6 324 wait(buttonrechts.read() == 1);
Zeekat 6:9c8e91bb1316 325 wait(0.3);
Zeekat 4:072b99947fc6 326 }
Zeekat 6:9c8e91bb1316 327 // reverse buttons
Zeekat 6:9c8e91bb1316 328 if(reverse_button_links.read() == 0)
Zeekat 6:9c8e91bb1316 329 {
Zeekat 6:9c8e91bb1316 330 reverse_links = !reverse_links;
Zeekat 6:9c8e91bb1316 331 wait(reverse_button_links.read() == 1);
Zeekat 6:9c8e91bb1316 332 wait(0.3);
Zeekat 6:9c8e91bb1316 333 }
Zeekat 6:9c8e91bb1316 334 if(reverse_button_rechts.read() == 0)
Zeekat 6:9c8e91bb1316 335 {
Zeekat 6:9c8e91bb1316 336 reverse_rechts = !reverse_rechts;
Zeekat 6:9c8e91bb1316 337 wait(reverse_button_rechts.read() == 1);
Zeekat 6:9c8e91bb1316 338 wait(0.3);
Zeekat 6:9c8e91bb1316 339 }
Zeekat 4:072b99947fc6 340 // Main Control stuff and options
Zeekat 4:072b99947fc6 341 if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops
Zeekat 2:2563df4ee829 342 {
Zeekat 5:67afe491a1e3 343 LED(1,1,0); // turn blue led on
Zeekat 4:072b99947fc6 344
Zeekat 4:072b99947fc6 345 if(motor1_go)
Zeekat 4:072b99947fc6 346 {
Zeekat 4:072b99947fc6 347 motor1_go = false;
Zeekat 4:072b99947fc6 348 motor1_control();
Zeekat 4:072b99947fc6 349 }
Zeekat 4:072b99947fc6 350 if(motor2_go)
Zeekat 4:072b99947fc6 351 {
Zeekat 4:072b99947fc6 352 motor2_go = false;
Zeekat 4:072b99947fc6 353 motor2_control();
Zeekat 4:072b99947fc6 354 }
Zeekat 2:2563df4ee829 355 }
Zeekat 4:072b99947fc6 356 if(loop_start == false && calib_start == true) // start calibration procedures
Zeekat 4:072b99947fc6 357 {
Zeekat 5:67afe491a1e3 358 LED(1,0,1); // turn green led on
Zeekat 4:072b99947fc6 359 }
Zeekat 4:072b99947fc6 360 if(loop_start == true && calib_start == true) // check if both buttons are enabled and stop everything
Zeekat 0:f6306e179750 361 {
Zeekat 5:67afe491a1e3 362 LED(0,1,1); // turn red led on
Zeekat 4:072b99947fc6 363 }
Zeekat 4:072b99947fc6 364 else
Zeekat 4:072b99947fc6 365 {
Zeekat 5:67afe491a1e3 366 LED(1,1,1); // turn leds off (both buttons false)
Zeekat 0:f6306e179750 367 }
Zeekat 0:f6306e179750 368 }
Zeekat 0:f6306e179750 369 }