Biorobotics_group_2 / Mbed 2 deprecated Motor_PID_set_parameters

Sets the Kp, Ki and Kd values of the PID controller for the encoder motors

Dependencies:   FastPWM HIDScope_motor_ff MODSERIAL QEI mbed

Fork of Encoder by Biorobotics_group_2

main.cpp@10:65f63a0a6b3c, 2016-10-31 (annotated)

Committer:
sjoerdbarts
Date:
Mon Oct 31 11:28:39 2016 +0000
Revision:
10:65f63a0a6b3c
Parent:
9:278d25dc0ef3 
Initial working code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sjoerdbarts 0:1883abafaa19 1 #include "mbed.h"
sjoerdbarts 8:fe907b9a0bab 2 #include "FastPWM.h"
sjoerdbarts 2:5fce9d33997f 3 #include "HIDScope.h"
sjoerdbarts 5:36788b154e25 4 #include "QEI.h"
sjoerdbarts 8:fe907b9a0bab 5 #include "BiQuad.h"
sjoerdbarts 3:ce0f979f15fb 6 #define SERIAL_BAUD 115200 // baud rate for serial communication
sjoerdbarts 8:fe907b9a0bab 7
sjoerdbarts 5:36788b154e25 8 Serial pc(USBTX,USBRX);
sjoerdbarts 3:ce0f979f15fb 9
sjoerdbarts 8:fe907b9a0bab 10 // Setup Pins
sjoerdbarts 8:fe907b9a0bab 11 // Note: Pin D10 and D11 for encoder, D4-D7 for motor controller
sjoerdbarts 9:278d25dc0ef3 12 // Potmeter 1 gives the reference position x
sjoerdbarts 9:278d25dc0ef3 13 AnalogIn pot1(A3);
sjoerdbarts 9:278d25dc0ef3 14 // Potmeter 2 gives the reference position y
sjoerdbarts 9:278d25dc0ef3 15 AnalogIn pot2(A4);
sjoerdbarts 8:fe907b9a0bab 16
sjoerdbarts 8:fe907b9a0bab 17 // Setup Buttons
sjoerdbarts 9:278d25dc0ef3 18 InterruptIn button1(PTB9); // button 1
sjoerdbarts 9:278d25dc0ef3 19 InterruptIn button2(PTA1); // button 2
sjoerdbarts 9:278d25dc0ef3 20 InterruptIn button3(PTC6); // SW2
sjoerdbarts 9:278d25dc0ef3 21 InterruptIn button4(PTA4); // SW3
sjoerdbarts 9:278d25dc0ef3 22
sjoerdbarts 9:278d25dc0ef3 23 //
sjoerdbarts 9:278d25dc0ef3 24 volatile bool button1_value = false;
sjoerdbarts 8:fe907b9a0bab 25
sjoerdbarts 8:fe907b9a0bab 26 // Set motor Pinouts
sjoerdbarts 8:fe907b9a0bab 27 DigitalOut motor1_dir(D4);
sjoerdbarts 8:fe907b9a0bab 28 FastPWM motor1_pwm(D5);
sjoerdbarts 9:278d25dc0ef3 29 DigitalOut motor2_dir(D7);
sjoerdbarts 9:278d25dc0ef3 30 FastPWM motor2_pwm(D6);
sjoerdbarts 8:fe907b9a0bab 31
sjoerdbarts 8:fe907b9a0bab 32 // Set LED pins
sjoerdbarts 8:fe907b9a0bab 33 DigitalOut led(LED_RED);
sjoerdbarts 8:fe907b9a0bab 34
sjoerdbarts 8:fe907b9a0bab 35 // Set HID scope
sjoerdbarts 9:278d25dc0ef3 36 HIDScope scope(6);
sjoerdbarts 8:fe907b9a0bab 37
sjoerdbarts 8:fe907b9a0bab 38 // Set encoder
sjoerdbarts 9:278d25dc0ef3 39 QEI m1_EncoderCW(D10,D11,NC,32);
sjoerdbarts 9:278d25dc0ef3 40 QEI m1_EncoderCCW(D11,D10,NC,32);
sjoerdbarts 9:278d25dc0ef3 41 QEI m2_EncoderCW(D13,D12,NC,32);
sjoerdbarts 9:278d25dc0ef3 42 QEI m2_EncoderCCW(D12,D13,NC,32);
sjoerdbarts 9:278d25dc0ef3 43
sjoerdbarts 9:278d25dc0ef3 44 volatile const int COUNTS_PER_REV = 8400;
sjoerdbarts 9:278d25dc0ef3 45 volatile const float DEGREES_PER_PULSE = 8400 / 360;
sjoerdbarts 9:278d25dc0ef3 46 volatile const float CONTROLLER_TS = 0.01; // 1000 Hz
sjoerdbarts 9:278d25dc0ef3 47
sjoerdbarts 9:278d25dc0ef3 48 // Set initial Kp and Ki
sjoerdbarts 9:278d25dc0ef3 49 volatile double Kp = 0.01; // last known good Kp: 0.01
sjoerdbarts 9:278d25dc0ef3 50 volatile double Ki = 0.0; // last known good Ki: 0.0025
sjoerdbarts 9:278d25dc0ef3 51 volatile double Kd = 0.0; // last known good Kd: 0.0
sjoerdbarts 9:278d25dc0ef3 52
sjoerdbarts 9:278d25dc0ef3 53 volatile double Ts = 0.01;
sjoerdbarts 9:278d25dc0ef3 54 volatile double N = 100;
sjoerdbarts 9:278d25dc0ef3 55
sjoerdbarts 9:278d25dc0ef3 56 // Memory values for controllers
sjoerdbarts 9:278d25dc0ef3 57 double m1_v1 = 0, m1_v2 = 0;
sjoerdbarts 9:278d25dc0ef3 58 double m2_v1 = 0, m2_v2 = 0;
sjoerdbarts 9:278d25dc0ef3 59
sjoerdbarts 9:278d25dc0ef3 60 // Set default mode
sjoerdbarts 9:278d25dc0ef3 61 volatile int Mode = 0;
sjoerdbarts 9:278d25dc0ef3 62
sjoerdbarts 9:278d25dc0ef3 63 // Variabelen voor MotorHoekBerekenen
sjoerdbarts 9:278d25dc0ef3 64 volatile double x = 0.0; // beginpositie x en y
sjoerdbarts 9:278d25dc0ef3 65 volatile double y = 305.5;
sjoerdbarts 9:278d25dc0ef3 66 volatile const double pi = 3.14159265359;
sjoerdbarts 9:278d25dc0ef3 67 volatile double Theta1Gear = 60.0; // Beginpositie op 60 graden
sjoerdbarts 9:278d25dc0ef3 68 volatile double Theta2Gear = 60.0;
sjoerdbarts 9:278d25dc0ef3 69 volatile double Theta1 = Theta1Gear*42/12; // Beginpositie van MotorHoek
sjoerdbarts 9:278d25dc0ef3 70 volatile double Theta2 = Theta2Gear*42/12; // frequentie in Hz waarmee theta wordt uigereken
sjoerdbarts 9:278d25dc0ef3 71
sjoerdbarts 9:278d25dc0ef3 72 // Functions for angle calculation
sjoerdbarts 9:278d25dc0ef3 73 double Calc_Prev_x () {
sjoerdbarts 9:278d25dc0ef3 74 double Prev_x = x;
sjoerdbarts 9:278d25dc0ef3 75 //pc.printf("prev x = %f\r\n", Prev_x);
sjoerdbarts 9:278d25dc0ef3 76 return Prev_x;
sjoerdbarts 9:278d25dc0ef3 77 }
sjoerdbarts 9:278d25dc0ef3 78
sjoerdbarts 9:278d25dc0ef3 79 // vorige y opslaan
sjoerdbarts 9:278d25dc0ef3 80 double Calc_Prev_y () {
sjoerdbarts 9:278d25dc0ef3 81 double Prev_y = y;
sjoerdbarts 9:278d25dc0ef3 82 //pc.printf("prev y = %f\r\n", Prev_y);
sjoerdbarts 9:278d25dc0ef3 83 return Prev_y;
sjoerdbarts 9:278d25dc0ef3 84 }
sjoerdbarts 9:278d25dc0ef3 85
sjoerdbarts 9:278d25dc0ef3 86 // vorige Theta1Gear opslaan
sjoerdbarts 9:278d25dc0ef3 87 double Calc_Prev_Theta1_Gear () {
sjoerdbarts 9:278d25dc0ef3 88 double Prev_Theta1_Gear = Theta1Gear;
sjoerdbarts 9:278d25dc0ef3 89 return Prev_Theta1_Gear;
sjoerdbarts 9:278d25dc0ef3 90 }
sjoerdbarts 9:278d25dc0ef3 91
sjoerdbarts 9:278d25dc0ef3 92 //vorige Theta2Gear opslaan
sjoerdbarts 9:278d25dc0ef3 93 double Calc_Prev_Theta2_Gear () {
sjoerdbarts 9:278d25dc0ef3 94 double Prev_Theta2_Gear = Theta2Gear;
sjoerdbarts 9:278d25dc0ef3 95 return Prev_Theta2_Gear;
sjoerdbarts 9:278d25dc0ef3 96 }
sjoerdbarts 8:fe907b9a0bab 97
sjoerdbarts 9:278d25dc0ef3 98 void CalcXY()
sjoerdbarts 9:278d25dc0ef3 99 {
sjoerdbarts 9:278d25dc0ef3 100 /*
sjoerdbarts 9:278d25dc0ef3 101 double Step = 5/Fr_Speed_Theta ; //10 mm per seconde afleggen
sjoerdbarts 9:278d25dc0ef3 102
sjoerdbarts 9:278d25dc0ef3 103 if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) {
sjoerdbarts 9:278d25dc0ef3 104 Stepper_State = true; // we zijn aan het flipper dus geen andere dingen doen
sjoerdbarts 9:278d25dc0ef3 105 FunctieFlipper() ;
sjoerdbarts 9:278d25dc0ef3 106 }
sjoerdbarts 9:278d25dc0ef3 107 else if (BicepsLeft==true && BicepsRight==false && Leg==false && Stepper_State==false) {
sjoerdbarts 9:278d25dc0ef3 108 x = x - Step;
sjoerdbarts 9:278d25dc0ef3 109 }
sjoerdbarts 9:278d25dc0ef3 110 else if (BicepsLeft==false && BicepsRight==true && Leg==false && Stepper_State==false) {
sjoerdbarts 9:278d25dc0ef3 111 x = x + Step; // naar Right bewegen
sjoerdbarts 9:278d25dc0ef3 112 }
sjoerdbarts 9:278d25dc0ef3 113 else if (BicepsLeft==true && BicepsRight==true && Leg==false && Stepper_State==false) {
sjoerdbarts 9:278d25dc0ef3 114 y = y + Step; // naar voren bewegen
sjoerdbarts 9:278d25dc0ef3 115 }
sjoerdbarts 9:278d25dc0ef3 116 else if (BicepsLeft==false && BicepsRight==true && Leg==true && Stepper_State==false) {
sjoerdbarts 9:278d25dc0ef3 117 y = y - Step; // naar achter bewegen
sjoerdbarts 9:278d25dc0ef3 118 }
sjoerdbarts 9:278d25dc0ef3 119 else if (BicepsLeft==false && BicepsRight==false && Leg==false || Stepper_State==true) {
sjoerdbarts 9:278d25dc0ef3 120 }
sjoerdbarts 9:278d25dc0ef3 121 */
sjoerdbarts 9:278d25dc0ef3 122
sjoerdbarts 9:278d25dc0ef3 123 x = (pot1.read()-0.5f)*400.0f; // x is een waarde tussen de -200 en 200
sjoerdbarts 9:278d25dc0ef3 124 y = 50.0f+(pot2.read()*256.0f);
sjoerdbarts 9:278d25dc0ef3 125
sjoerdbarts 9:278d25dc0ef3 126 // Grenswaardes LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
sjoerdbarts 9:278d25dc0ef3 127 if (x > 200) {
sjoerdbarts 9:278d25dc0ef3 128 x = 200;
sjoerdbarts 9:278d25dc0ef3 129 }
sjoerdbarts 9:278d25dc0ef3 130 else if (x < -200) {
sjoerdbarts 9:278d25dc0ef3 131 x = -200;
sjoerdbarts 9:278d25dc0ef3 132 }
sjoerdbarts 9:278d25dc0ef3 133 if (y > 306) {
sjoerdbarts 9:278d25dc0ef3 134 y = 306;
sjoerdbarts 9:278d25dc0ef3 135 }
sjoerdbarts 9:278d25dc0ef3 136 else if (y < 50) {
sjoerdbarts 9:278d25dc0ef3 137 y = 50; // GOKJE, UITPROBEREN
sjoerdbarts 9:278d25dc0ef3 138 }
sjoerdbarts 9:278d25dc0ef3 139 //pc.printf("x = %f, y = %f\r\n", x, y);
sjoerdbarts 9:278d25dc0ef3 140
sjoerdbarts 9:278d25dc0ef3 141 //scope.set(2,x);
sjoerdbarts 9:278d25dc0ef3 142 //scope.set(3,y);
sjoerdbarts 9:278d25dc0ef3 143 }
sjoerdbarts 9:278d25dc0ef3 144
sjoerdbarts 9:278d25dc0ef3 145 // diagonaal berekenen voor linker arm
sjoerdbarts 9:278d25dc0ef3 146 double CalcDia1 (double x, double y) {
sjoerdbarts 9:278d25dc0ef3 147 double a = 50.0; // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden) KEERTJE NAMETEN
sjoerdbarts 9:278d25dc0ef3 148 double BV1 = sqrt(pow((a+x),2) + pow(y,2)); // diagonaal (afstand van armas tot locatie) berekenen
sjoerdbarts 9:278d25dc0ef3 149 double Dia1 = pow(BV1,2)/(2*BV1); // berekenen van de afstand oorsprong tot diagonaal
sjoerdbarts 9:278d25dc0ef3 150
sjoerdbarts 9:278d25dc0ef3 151 //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia1, x, y);
sjoerdbarts 9:278d25dc0ef3 152 return Dia1;
sjoerdbarts 9:278d25dc0ef3 153 }
sjoerdbarts 9:278d25dc0ef3 154
sjoerdbarts 9:278d25dc0ef3 155 // diagonaal berekenen voor rechter arm
sjoerdbarts 9:278d25dc0ef3 156 double CalcDia2 (double x, double y) {
sjoerdbarts 9:278d25dc0ef3 157 double a = 50.0;
sjoerdbarts 9:278d25dc0ef3 158 double BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm
sjoerdbarts 9:278d25dc0ef3 159 double Dia2 = pow(BV2,2)/(2*BV2);
sjoerdbarts 9:278d25dc0ef3 160
sjoerdbarts 9:278d25dc0ef3 161 //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia2, x, y);
sjoerdbarts 9:278d25dc0ef3 162 return Dia2;
sjoerdbarts 9:278d25dc0ef3 163 }
sjoerdbarts 9:278d25dc0ef3 164
sjoerdbarts 9:278d25dc0ef3 165 // calculate Theta1
sjoerdbarts 9:278d25dc0ef3 166 void CalcTheta1 (double Dia1) {
sjoerdbarts 9:278d25dc0ef3 167 double a = 50.0;
sjoerdbarts 9:278d25dc0ef3 168 double Bar = 200.0; // lengte van de armen
sjoerdbarts 9:278d25dc0ef3 169
sjoerdbarts 9:278d25dc0ef3 170 // Hoek berekenen van het grote tandwiel (gear)
sjoerdbarts 9:278d25dc0ef3 171 if (x > -a) {
sjoerdbarts 9:278d25dc0ef3 172 Theta1Gear = pi - atan(y/(x+a)) - acos(Dia1/Bar);
sjoerdbarts 9:278d25dc0ef3 173 }
sjoerdbarts 9:278d25dc0ef3 174 else if (x > -a) {
sjoerdbarts 9:278d25dc0ef3 175 Theta1Gear = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
sjoerdbarts 9:278d25dc0ef3 176 }
sjoerdbarts 9:278d25dc0ef3 177 else { // als x=-a
sjoerdbarts 9:278d25dc0ef3 178 Theta1Gear = 0.5*pi - acos(Dia1/Bar);
sjoerdbarts 9:278d25dc0ef3 179 }
sjoerdbarts 9:278d25dc0ef3 180 Theta1Gear = Theta1Gear*180.0/pi; // veranderen van radialen naar graden
sjoerdbarts 9:278d25dc0ef3 181
sjoerdbarts 9:278d25dc0ef3 182 // omrekenen van grote tandwiel naar motortandwiel
sjoerdbarts 9:278d25dc0ef3 183 Theta1 = Theta1Gear*42.0/12.0; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
sjoerdbarts 9:278d25dc0ef3 184
sjoerdbarts 9:278d25dc0ef3 185 // pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta1, Theta1Gear);
sjoerdbarts 9:278d25dc0ef3 186 }
sjoerdbarts 7:e7aa4f10d1fb 187
sjoerdbarts 9:278d25dc0ef3 188 void CalcTheta2 (double Dia2) {
sjoerdbarts 9:278d25dc0ef3 189 double a = 50.0;
sjoerdbarts 9:278d25dc0ef3 190 double Bar = 200.0; // lengte van de armen
sjoerdbarts 9:278d25dc0ef3 191 double Prev_Theta2_Gear = Theta2Gear;
sjoerdbarts 9:278d25dc0ef3 192
sjoerdbarts 9:278d25dc0ef3 193 // Hoek berekenen van het grote tandwiel (gear)
sjoerdbarts 9:278d25dc0ef3 194 if (x < a) {
sjoerdbarts 9:278d25dc0ef3 195 Theta2Gear = pi + atan(y/(x-a)) - acos(Dia2/Bar);
sjoerdbarts 9:278d25dc0ef3 196 }
sjoerdbarts 9:278d25dc0ef3 197 else if (x > a) {
sjoerdbarts 9:278d25dc0ef3 198 Theta2Gear = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
sjoerdbarts 9:278d25dc0ef3 199 }
sjoerdbarts 9:278d25dc0ef3 200 else { // als x=a
sjoerdbarts 9:278d25dc0ef3 201 Theta2Gear = 0.5*pi - acos(Dia2/Bar);
sjoerdbarts 9:278d25dc0ef3 202 }
sjoerdbarts 9:278d25dc0ef3 203 Theta2Gear = Theta2Gear*180/pi; // veranderen van radialen naar graden
sjoerdbarts 9:278d25dc0ef3 204
sjoerdbarts 9:278d25dc0ef3 205 // omrekenen van grote tandwiel naar motortandwiel
sjoerdbarts 9:278d25dc0ef3 206 Theta2 = Theta2Gear*42.0/12.0; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
sjoerdbarts 9:278d25dc0ef3 207
sjoerdbarts 9:278d25dc0ef3 208 // pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
sjoerdbarts 9:278d25dc0ef3 209 }
sjoerdbarts 9:278d25dc0ef3 210
sjoerdbarts 9:278d25dc0ef3 211 // als een van de hoeken te groot wordt, zet dan alles terug naar de vorige positie
sjoerdbarts 9:278d25dc0ef3 212 void AngleLimits (double Prev_Theta1_Gear, double Prev_Theta2_Gear, double Prev_x, double Prev_y) {
sjoerdbarts 9:278d25dc0ef3 213 double MaxThetaGear = 100.0; // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
sjoerdbarts 9:278d25dc0ef3 214
sjoerdbarts 9:278d25dc0ef3 215 if (Theta1Gear >= MaxThetaGear || Theta2Gear >= MaxThetaGear) {
sjoerdbarts 9:278d25dc0ef3 216 Theta1Gear = Prev_Theta1_Gear;
sjoerdbarts 9:278d25dc0ef3 217 Theta2Gear = Prev_Theta2_Gear;
sjoerdbarts 9:278d25dc0ef3 218 x = Prev_x;
sjoerdbarts 9:278d25dc0ef3 219 y = Prev_y;
sjoerdbarts 9:278d25dc0ef3 220
sjoerdbarts 9:278d25dc0ef3 221 Theta1 = Theta1Gear*42.0/12.0;
sjoerdbarts 9:278d25dc0ef3 222 Theta2 = Theta2Gear*42.0/12.0;
sjoerdbarts 9:278d25dc0ef3 223 }
sjoerdbarts 9:278d25dc0ef3 224 // pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
sjoerdbarts 9:278d25dc0ef3 225 }
sjoerdbarts 9:278d25dc0ef3 226
sjoerdbarts 9:278d25dc0ef3 227 void CalculationsForTheta () {
sjoerdbarts 9:278d25dc0ef3 228 double Prev_x = Calc_Prev_x ();
sjoerdbarts 9:278d25dc0ef3 229 double Prev_y = Calc_Prev_y ();
sjoerdbarts 10:65f63a0a6b3c 230 double Prev_Theta1_Gear = Calc_Prev_Theta1_Gear ();
sjoerdbarts 10:65f63a0a6b3c 231 double Prev_Theta2_Gear = Calc_Prev_Theta2_Gear ();
sjoerdbarts 9:278d25dc0ef3 232 CalcXY();
sjoerdbarts 9:278d25dc0ef3 233 double Dia1 = CalcDia1 (x, y);
sjoerdbarts 9:278d25dc0ef3 234 double Dia2 = CalcDia2 (x, y);
sjoerdbarts 9:278d25dc0ef3 235 CalcTheta1 (Dia1);
sjoerdbarts 9:278d25dc0ef3 236 CalcTheta2 (Dia2);
sjoerdbarts 10:65f63a0a6b3c 237 AngleLimits (Prev_Theta1_Gear, Prev_Theta2_Gear, Prev_x, Prev_y); // laatste check
sjoerdbarts 9:278d25dc0ef3 238 }
sjoerdbarts 9:278d25dc0ef3 239
sjoerdbarts 9:278d25dc0ef3 240 void Set_Mode()
sjoerdbarts 9:278d25dc0ef3 241 {
sjoerdbarts 9:278d25dc0ef3 242 Mode++;
sjoerdbarts 9:278d25dc0ef3 243 if (Mode == 3)
sjoerdbarts 9:278d25dc0ef3 244 {
sjoerdbarts 9:278d25dc0ef3 245 Mode = 0;
sjoerdbarts 9:278d25dc0ef3 246 }
sjoerdbarts 9:278d25dc0ef3 247 pc.printf("\r 0: Kp \r\n");
sjoerdbarts 9:278d25dc0ef3 248 pc.printf("\r 1: Ki \r\n");
sjoerdbarts 9:278d25dc0ef3 249 pc.printf("\r 2: Kd \r\n");
sjoerdbarts 9:278d25dc0ef3 250 pc.printf("\r MODE = %i \r\n",Mode);
sjoerdbarts 9:278d25dc0ef3 251 }
sjoerdbarts 9:278d25dc0ef3 252
sjoerdbarts 9:278d25dc0ef3 253 void increase()
sjoerdbarts 9:278d25dc0ef3 254 {
sjoerdbarts 9:278d25dc0ef3 255 switch(Mode)
sjoerdbarts 9:278d25dc0ef3 256 {
sjoerdbarts 9:278d25dc0ef3 257 case 0:
sjoerdbarts 9:278d25dc0ef3 258 Kp = Kp + 0.01f;
sjoerdbarts 9:278d25dc0ef3 259 break;
sjoerdbarts 9:278d25dc0ef3 260 case 1:
sjoerdbarts 9:278d25dc0ef3 261 Ki = Ki + 0.00001f;
sjoerdbarts 9:278d25dc0ef3 262 break;
sjoerdbarts 9:278d25dc0ef3 263 case 2:
sjoerdbarts 9:278d25dc0ef3 264 Kd = Kd + 0.01f;
sjoerdbarts 9:278d25dc0ef3 265 break;
sjoerdbarts 9:278d25dc0ef3 266 default:
sjoerdbarts 9:278d25dc0ef3 267 pc.printf("\r swtich error \r\n");
sjoerdbarts 9:278d25dc0ef3 268 }
sjoerdbarts 9:278d25dc0ef3 269 pc.printf("\r Kp: %f, Ki: %f, Kd: %f \r\n",Kp,Ki,Kd);
sjoerdbarts 9:278d25dc0ef3 270 }
sjoerdbarts 8:fe907b9a0bab 271
sjoerdbarts 9:278d25dc0ef3 272 void decrease()
sjoerdbarts 9:278d25dc0ef3 273 {
sjoerdbarts 9:278d25dc0ef3 274 switch(Mode)
sjoerdbarts 9:278d25dc0ef3 275 {
sjoerdbarts 9:278d25dc0ef3 276 case 0:
sjoerdbarts 9:278d25dc0ef3 277 if (Kp <= 0.0f)
sjoerdbarts 9:278d25dc0ef3 278 {
sjoerdbarts 9:278d25dc0ef3 279 Kp = 0.0f;
sjoerdbarts 9:278d25dc0ef3 280 break;
sjoerdbarts 9:278d25dc0ef3 281 }
sjoerdbarts 9:278d25dc0ef3 282 Kp = Kp - 0.01f;
sjoerdbarts 9:278d25dc0ef3 283 break;
sjoerdbarts 9:278d25dc0ef3 284 case 1:
sjoerdbarts 9:278d25dc0ef3 285 if (Ki == 0.0f)
sjoerdbarts 9:278d25dc0ef3 286 {
sjoerdbarts 9:278d25dc0ef3 287 Ki = 0.0f;
sjoerdbarts 9:278d25dc0ef3 288 break;
sjoerdbarts 9:278d25dc0ef3 289 }
sjoerdbarts 9:278d25dc0ef3 290 Ki = Ki - 0.00001f;
sjoerdbarts 9:278d25dc0ef3 291 break;
sjoerdbarts 9:278d25dc0ef3 292 case 2:
sjoerdbarts 9:278d25dc0ef3 293 if (Kd == 0.0f)
sjoerdbarts 9:278d25dc0ef3 294 {
sjoerdbarts 9:278d25dc0ef3 295 Kd = 0.0f;
sjoerdbarts 9:278d25dc0ef3 296 break;
sjoerdbarts 9:278d25dc0ef3 297 }
sjoerdbarts 9:278d25dc0ef3 298 Kd = Kd - 0.01f;
sjoerdbarts 9:278d25dc0ef3 299 break;
sjoerdbarts 9:278d25dc0ef3 300 default:
sjoerdbarts 9:278d25dc0ef3 301 pc.printf("\r swtich error \r\n");
sjoerdbarts 9:278d25dc0ef3 302 }
sjoerdbarts 9:278d25dc0ef3 303 pc.printf("\r Kp: %f, Ki: %f, Kd: %f \r\n",Kp,Ki,Kd);
sjoerdbarts 9:278d25dc0ef3 304 }
sjoerdbarts 9:278d25dc0ef3 305
sjoerdbarts 9:278d25dc0ef3 306 double m1_GetPosition()
sjoerdbarts 9:278d25dc0ef3 307 {
sjoerdbarts 9:278d25dc0ef3 308 int countsCW = m1_EncoderCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 309 int countsCCW= m1_EncoderCCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 310 int net_counts=countsCW-countsCCW;
sjoerdbarts 9:278d25dc0ef3 311 double Position=(net_counts*360.0)/COUNTS_PER_REV;
sjoerdbarts 9:278d25dc0ef3 312 return Position;
sjoerdbarts 9:278d25dc0ef3 313 }
sjoerdbarts 6:ed11342ab079 314
sjoerdbarts 9:278d25dc0ef3 315 double m2_GetPosition()
sjoerdbarts 9:278d25dc0ef3 316 {
sjoerdbarts 9:278d25dc0ef3 317 int countsCW = m2_EncoderCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 318 int countsCCW= m2_EncoderCCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 319 int net_counts=countsCW-countsCCW;
sjoerdbarts 9:278d25dc0ef3 320 double Position=(net_counts*360.0)/COUNTS_PER_REV;
sjoerdbarts 9:278d25dc0ef3 321 return Position;
sjoerdbarts 9:278d25dc0ef3 322 }
sjoerdbarts 9:278d25dc0ef3 323
sjoerdbarts 9:278d25dc0ef3 324 double m1_GetPosition_cal()
sjoerdbarts 9:278d25dc0ef3 325 {
sjoerdbarts 9:278d25dc0ef3 326 int countsCW = m1_EncoderCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 327 int countsCCW= m1_EncoderCCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 328 int net_counts=countsCW-countsCCW;
sjoerdbarts 9:278d25dc0ef3 329 double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
sjoerdbarts 9:278d25dc0ef3 330 return Position;
sjoerdbarts 9:278d25dc0ef3 331 }
sjoerdbarts 7:e7aa4f10d1fb 332
sjoerdbarts 9:278d25dc0ef3 333 double m2_GetPosition_cal()
sjoerdbarts 9:278d25dc0ef3 334 {
sjoerdbarts 9:278d25dc0ef3 335 int countsCW = m2_EncoderCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 336 int countsCCW= m2_EncoderCCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 337 int net_counts=countsCW-countsCCW;
sjoerdbarts 9:278d25dc0ef3 338 double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
sjoerdbarts 9:278d25dc0ef3 339 return Position;
sjoerdbarts 9:278d25dc0ef3 340 }
sjoerdbarts 9:278d25dc0ef3 341 double m1_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m1_v1, double &m1_v2)
sjoerdbarts 9:278d25dc0ef3 342 {
sjoerdbarts 9:278d25dc0ef3 343 double a1 = -4/(N*Ts+2),
sjoerdbarts 9:278d25dc0ef3 344 a2 = -1*(N*Ts - 2)/(N*Ts+2),
sjoerdbarts 9:278d25dc0ef3 345 b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
sjoerdbarts 9:278d25dc0ef3 346 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
sjoerdbarts 9:278d25dc0ef3 347 b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
sjoerdbarts 9:278d25dc0ef3 348
sjoerdbarts 9:278d25dc0ef3 349 double v = error - a1*m1_v1 - a2*m1_v2;
sjoerdbarts 9:278d25dc0ef3 350 double u = b0*v + b1*m1_v1 + b2*m1_v2;
sjoerdbarts 9:278d25dc0ef3 351 m1_v2 = m1_v1; m1_v1 = v;
sjoerdbarts 9:278d25dc0ef3 352 return u;
sjoerdbarts 9:278d25dc0ef3 353 }
sjoerdbarts 9:278d25dc0ef3 354
sjoerdbarts 9:278d25dc0ef3 355 double m2_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m2_v1, double &m2_v2)
sjoerdbarts 9:278d25dc0ef3 356 {
sjoerdbarts 9:278d25dc0ef3 357 double a1 = -4/(N*Ts+2),
sjoerdbarts 9:278d25dc0ef3 358 a2 = -1*(N*Ts - 2)/(N*Ts+2),
sjoerdbarts 9:278d25dc0ef3 359 b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
sjoerdbarts 9:278d25dc0ef3 360 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
sjoerdbarts 9:278d25dc0ef3 361 b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
sjoerdbarts 9:278d25dc0ef3 362
sjoerdbarts 9:278d25dc0ef3 363 double v = error - a1*m1_v1 - a2*m1_v2;
sjoerdbarts 9:278d25dc0ef3 364 double u = b0*v + b1*m1_v1 + b2*m1_v2;
sjoerdbarts 9:278d25dc0ef3 365 m1_v2 = m1_v1; m1_v1 = v;
sjoerdbarts 9:278d25dc0ef3 366 return u;
sjoerdbarts 9:278d25dc0ef3 367 }
sjoerdbarts 8:fe907b9a0bab 368
sjoerdbarts 8:fe907b9a0bab 369
sjoerdbarts 0:1883abafaa19 370
sjoerdbarts 9:278d25dc0ef3 371 void SetMotor(int motor_number, double MotorValue)
sjoerdbarts 0:1883abafaa19 372 {
sjoerdbarts 8:fe907b9a0bab 373 // Given -1<=motorValue<=1, this sets the PWM and direction
sjoerdbarts 8:fe907b9a0bab 374 // bits for motor 1. Positive value makes motor rotating
sjoerdbarts 8:fe907b9a0bab 375 // clockwise. motorValues outside range are truncated to
sjoerdbarts 8:fe907b9a0bab 376 // within range
sjoerdbarts 9:278d25dc0ef3 377 if (motor_number == 1)
sjoerdbarts 9:278d25dc0ef3 378 {
sjoerdbarts 9:278d25dc0ef3 379 if (MotorValue >=0)
sjoerdbarts 9:278d25dc0ef3 380 {
sjoerdbarts 9:278d25dc0ef3 381 motor1_dir=0;
sjoerdbarts 9:278d25dc0ef3 382 }
sjoerdbarts 9:278d25dc0ef3 383 else
sjoerdbarts 9:278d25dc0ef3 384 {
sjoerdbarts 9:278d25dc0ef3 385 motor1_dir=1;
sjoerdbarts 9:278d25dc0ef3 386 }
sjoerdbarts 9:278d25dc0ef3 387 if (fabs(MotorValue)>1){
sjoerdbarts 9:278d25dc0ef3 388 motor1_pwm.write(1);
sjoerdbarts 9:278d25dc0ef3 389 }
sjoerdbarts 9:278d25dc0ef3 390 else
sjoerdbarts 9:278d25dc0ef3 391 {
sjoerdbarts 9:278d25dc0ef3 392 motor1_pwm.write(abs(MotorValue));
sjoerdbarts 9:278d25dc0ef3 393 }
sjoerdbarts 8:fe907b9a0bab 394 }
sjoerdbarts 9:278d25dc0ef3 395 else
sjoerdbarts 9:278d25dc0ef3 396 {
sjoerdbarts 9:278d25dc0ef3 397 if (MotorValue >=0)
sjoerdbarts 9:278d25dc0ef3 398 {
sjoerdbarts 9:278d25dc0ef3 399 motor2_dir=0;
sjoerdbarts 9:278d25dc0ef3 400 }
sjoerdbarts 9:278d25dc0ef3 401 else
sjoerdbarts 9:278d25dc0ef3 402 {
sjoerdbarts 9:278d25dc0ef3 403 motor2_dir=1;
sjoerdbarts 9:278d25dc0ef3 404 }
sjoerdbarts 9:278d25dc0ef3 405 if (fabs(MotorValue)>1){
sjoerdbarts 9:278d25dc0ef3 406 motor2_pwm.write(1);
sjoerdbarts 9:278d25dc0ef3 407 }
sjoerdbarts 9:278d25dc0ef3 408 else
sjoerdbarts 9:278d25dc0ef3 409 {
sjoerdbarts 9:278d25dc0ef3 410 motor2_pwm.write(abs(MotorValue));
sjoerdbarts 9:278d25dc0ef3 411 }
sjoerdbarts 9:278d25dc0ef3 412 }
sjoerdbarts 8:fe907b9a0bab 413 }
sjoerdbarts 8:fe907b9a0bab 414
sjoerdbarts 8:fe907b9a0bab 415 void BlinkLed(){
sjoerdbarts 8:fe907b9a0bab 416 led = not led;
sjoerdbarts 8:fe907b9a0bab 417 }
sjoerdbarts 8:fe907b9a0bab 418
sjoerdbarts 9:278d25dc0ef3 419 void Controller_motor()
sjoerdbarts 9:278d25dc0ef3 420 {
sjoerdbarts 9:278d25dc0ef3 421 // calculate the reference position
sjoerdbarts 9:278d25dc0ef3 422 CalculationsForTheta();
sjoerdbarts 9:278d25dc0ef3 423 // get the actual position
sjoerdbarts 9:278d25dc0ef3 424 double m1_Position = m1_GetPosition_cal();
sjoerdbarts 9:278d25dc0ef3 425 double m2_Position = m2_GetPosition_cal();
sjoerdbarts 9:278d25dc0ef3 426 // Set position scopes
sjoerdbarts 9:278d25dc0ef3 427 scope.set(0,x);
sjoerdbarts 9:278d25dc0ef3 428 scope.set(1,y);
sjoerdbarts 9:278d25dc0ef3 429 scope.set(2,Theta1);
sjoerdbarts 9:278d25dc0ef3 430 scope.set(3,Theta2);
sjoerdbarts 9:278d25dc0ef3 431 /*
sjoerdbarts 9:278d25dc0ef3 432 scope.set(0,Theta1);
sjoerdbarts 9:278d25dc0ef3 433 scope.set(1,m1_Position);
sjoerdbarts 9:278d25dc0ef3 434 scope.set(3,Theta2);
sjoerdbarts 9:278d25dc0ef3 435 scope.set(4,m2_Position);
sjoerdbarts 9:278d25dc0ef3 436 */
sjoerdbarts 9:278d25dc0ef3 437 //scope.set(2,m1_Position);
sjoerdbarts 9:278d25dc0ef3 438 //scope.set(4,m2_Position);
sjoerdbarts 9:278d25dc0ef3 439 // calc the error
sjoerdbarts 9:278d25dc0ef3 440 double m1_error = Theta1 - m1_Position;
sjoerdbarts 9:278d25dc0ef3 441 double m2_error = Theta2 - m2_Position;
sjoerdbarts 9:278d25dc0ef3 442 //scope.set(2,m1_error);
sjoerdbarts 9:278d25dc0ef3 443 //scope.set(6,m2_error);
sjoerdbarts 9:278d25dc0ef3 444 // calc motorvalues for controller;
sjoerdbarts 9:278d25dc0ef3 445 double m1_MotorValue = m1_PID(m1_error, Kp, Ki, Kd, Ts, N, m1_v1, m1_v2);
sjoerdbarts 9:278d25dc0ef3 446 double m2_MotorValue = m2_PID(m2_error, Kp, Ki, Kd, Ts, N, m2_v1, m2_v2);
sjoerdbarts 9:278d25dc0ef3 447 scope.set(4,m1_MotorValue);
sjoerdbarts 9:278d25dc0ef3 448 scope.set(5,m2_MotorValue);
sjoerdbarts 9:278d25dc0ef3 449 // Set the motorvalues
sjoerdbarts 9:278d25dc0ef3 450 SetMotor(1, m1_MotorValue);
sjoerdbarts 9:278d25dc0ef3 451 SetMotor(2, m2_MotorValue);
sjoerdbarts 9:278d25dc0ef3 452 // Set motorvalues for scope
sjoerdbarts 9:278d25dc0ef3 453 // Send data to HIDScope
sjoerdbarts 9:278d25dc0ef3 454 scope.send();
sjoerdbarts 9:278d25dc0ef3 455 }
sjoerdbarts 9:278d25dc0ef3 456
sjoerdbarts 9:278d25dc0ef3 457 void button1_switch()
sjoerdbarts 9:278d25dc0ef3 458 {
sjoerdbarts 9:278d25dc0ef3 459 button1_value = not button1_value;
sjoerdbarts 9:278d25dc0ef3 460 }
sjoerdbarts 9:278d25dc0ef3 461
sjoerdbarts 9:278d25dc0ef3 462 void PotControl()
sjoerdbarts 9:278d25dc0ef3 463 {
sjoerdbarts 9:278d25dc0ef3 464 double Motor1_Value = (pot1.read() - 0.5f)/2.0f;
sjoerdbarts 9:278d25dc0ef3 465 double Motor2_Value = (pot2.read() - 0.5f)/2.0f;
sjoerdbarts 9:278d25dc0ef3 466 //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
sjoerdbarts 9:278d25dc0ef3 467 //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
sjoerdbarts 9:278d25dc0ef3 468 double m1_Position = m1_GetPosition();
sjoerdbarts 9:278d25dc0ef3 469 double m2_Position = m2_GetPosition();
sjoerdbarts 9:278d25dc0ef3 470
sjoerdbarts 9:278d25dc0ef3 471
sjoerdbarts 9:278d25dc0ef3 472 scope.set(0, Motor1_Value);
sjoerdbarts 9:278d25dc0ef3 473 scope.set(1, m1_Position);
sjoerdbarts 9:278d25dc0ef3 474 scope.set(2, Motor2_Value);
sjoerdbarts 9:278d25dc0ef3 475 scope.set(3, m2_Position);
sjoerdbarts 9:278d25dc0ef3 476 scope.send();
sjoerdbarts 9:278d25dc0ef3 477 // Write the motors
sjoerdbarts 9:278d25dc0ef3 478 SetMotor(1, Motor1_Value);
sjoerdbarts 9:278d25dc0ef3 479 SetMotor(2, Motor2_Value);
sjoerdbarts 9:278d25dc0ef3 480 }
sjoerdbarts 9:278d25dc0ef3 481
sjoerdbarts 9:278d25dc0ef3 482 /*
sjoerdbarts 8:fe907b9a0bab 483 void CalculateSpeed() {
sjoerdbarts 5:36788b154e25 484 countsCW = EncoderCW.getPulses();
sjoerdbarts 5:36788b154e25 485 countsCCW= EncoderCCW.getPulses();
sjoerdbarts 5:36788b154e25 486 net_counts=countsCW-countsCCW;
sjoerdbarts 5:36788b154e25 487 degrees=(net_counts*360.0)/counts_per_rev;
sjoerdbarts 5:36788b154e25 488
sjoerdbarts 8:fe907b9a0bab 489 curr_degrees = degrees;
sjoerdbarts 8:fe907b9a0bab 490 speed = (curr_degrees-prev_degrees)/T_CalculateSpeed;
sjoerdbarts 8:fe907b9a0bab 491 prev_degrees = curr_degrees;
sjoerdbarts 8:fe907b9a0bab 492
sjoerdbarts 8:fe907b9a0bab 493 //scope.set(0, degrees);
sjoerdbarts 8:fe907b9a0bab 494 scope.set(0, speed);
sjoerdbarts 8:fe907b9a0bab 495 double speed_filtered = bqc.step(speed);
sjoerdbarts 8:fe907b9a0bab 496 scope.set(1,speed_filtered);
sjoerdbarts 8:fe907b9a0bab 497 scope.send();
sjoerdbarts 8:fe907b9a0bab 498 }
sjoerdbarts 9:278d25dc0ef3 499 */
sjoerdbarts 8:fe907b9a0bab 500
sjoerdbarts 8:fe907b9a0bab 501 int main(){
sjoerdbarts 8:fe907b9a0bab 502 // Set baud connection with PC
sjoerdbarts 8:fe907b9a0bab 503 pc.baud(SERIAL_BAUD);
sjoerdbarts 8:fe907b9a0bab 504 pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
sjoerdbarts 8:fe907b9a0bab 505
sjoerdbarts 8:fe907b9a0bab 506 // Setup Blinking LED
sjoerdbarts 8:fe907b9a0bab 507 led = 1;
sjoerdbarts 8:fe907b9a0bab 508 Ticker TickerBlinkLed;
sjoerdbarts 8:fe907b9a0bab 509 TickerBlinkLed.attach(BlinkLed,0.5);
sjoerdbarts 8:fe907b9a0bab 510
sjoerdbarts 8:fe907b9a0bab 511 // Set motor PWM speeds
sjoerdbarts 8:fe907b9a0bab 512 motor1_pwm.period(1.0/1000);
sjoerdbarts 9:278d25dc0ef3 513 motor2_pwm.period(1.0/1000);
sjoerdbarts 8:fe907b9a0bab 514
sjoerdbarts 9:278d25dc0ef3 515 // Setup Interruptin.fall
sjoerdbarts 9:278d25dc0ef3 516 button1.fall(button1_switch);
sjoerdbarts 9:278d25dc0ef3 517 button2.fall(Set_Mode);
sjoerdbarts 9:278d25dc0ef3 518 button3.fall(increase);
sjoerdbarts 9:278d25dc0ef3 519 button4.fall(decrease);
sjoerdbarts 9:278d25dc0ef3 520 // Setup motor control
sjoerdbarts 9:278d25dc0ef3 521 Ticker PIDControllerTicker;
sjoerdbarts 9:278d25dc0ef3 522 Ticker PotControlTicker;
sjoerdbarts 9:278d25dc0ef3 523
sjoerdbarts 9:278d25dc0ef3 524 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 525 pc.printf("\r\n Press button SW3 to start positioning the arms using PotControl\r\n");
sjoerdbarts 9:278d25dc0ef3 526 pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
sjoerdbarts 9:278d25dc0ef3 527 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 528 while (button1_value == false){}
sjoerdbarts 9:278d25dc0ef3 529 PotControlTicker.attach(&PotControl,0.01f);
sjoerdbarts 8:fe907b9a0bab 530
sjoerdbarts 9:278d25dc0ef3 531 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 532 pc.printf("\r\n When done positioning, press button SW3 to detach Potcontrol");
sjoerdbarts 9:278d25dc0ef3 533 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 534 while (button1_value == true){}
sjoerdbarts 9:278d25dc0ef3 535
sjoerdbarts 9:278d25dc0ef3 536 // Detach potcontrol
sjoerdbarts 9:278d25dc0ef3 537 PotControlTicker.detach();
sjoerdbarts 9:278d25dc0ef3 538
sjoerdbarts 9:278d25dc0ef3 539 // Set motors to 0
sjoerdbarts 9:278d25dc0ef3 540 motor2_pwm.write(0);
sjoerdbarts 9:278d25dc0ef3 541 motor2_pwm.write(0);
sjoerdbarts 8:fe907b9a0bab 542
sjoerdbarts 9:278d25dc0ef3 543 // Reset the encoders to set the 0 position
sjoerdbarts 9:278d25dc0ef3 544 m1_EncoderCW.reset();
sjoerdbarts 9:278d25dc0ef3 545 m1_EncoderCCW.reset();
sjoerdbarts 9:278d25dc0ef3 546 m2_EncoderCW.reset();
sjoerdbarts 9:278d25dc0ef3 547 m2_EncoderCCW.reset();
sjoerdbarts 9:278d25dc0ef3 548
sjoerdbarts 9:278d25dc0ef3 549 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 550 pc.printf("\r\n When done positioning, press button SW3 to start potmeter PID control");
sjoerdbarts 9:278d25dc0ef3 551 pc.printf("\r\n Make sure both potentiometers are positioned halfway!!! \r\n");
sjoerdbarts 9:278d25dc0ef3 552 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 553 while (button1_value == false){}
sjoerdbarts 9:278d25dc0ef3 554 PIDControllerTicker.attach(&Controller_motor, CONTROLLER_TS); // 100 Hz
sjoerdbarts 9:278d25dc0ef3 555 while(true){}
sjoerdbarts 0:1883abafaa19 556 }