Wouter Bos / Mbed 2 deprecated 005-motor_x-y_control

werkend x-y control

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@0:a643b1b38abe, 2015-10-18 (annotated)

Committer:
Zeekat
Date:
Sun Oct 18 17:27:35 2015 +0000
Revision:
0:a643b1b38abe
Child:
1:f3910e46b831
start version, copy of pot PID control

Who changed what in which revision?

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