Biorobotics_group_2 / Mbed 2 deprecated Encoder

Reads the encoder signals from 1 motor and displays the counts and degrees of the output shaft in Serial.

Dependencies:   HIDScope_motor_ff QEI mbed FastPWM MODSERIAL

Fork of HID_scope_test by Biorobotics_group_2

main.cpp@9:278d25dc0ef3, 2016-10-28 (annotated)

Committer:
sjoerdbarts
Date:
Fri Oct 28 09:58:22 2016 +0000
Revision:
9:278d25dc0ef3
Parent:
8:fe907b9a0bab 
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 9:278d25dc0ef3 230 double Calc_Prev_Theta1_Gear ();
sjoerdbarts 9:278d25dc0ef3 231 double 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 9:278d25dc0ef3 237 AngleLimits (Theta1Gear, Theta2Gear, Prev_x, Prev_y); // laatste check
sjoerdbarts 9:278d25dc0ef3 238 // hier mag je de motor gaan aansturen
sjoerdbarts 9:278d25dc0ef3 239 //scope.send();
sjoerdbarts 9:278d25dc0ef3 240 }
sjoerdbarts 9:278d25dc0ef3 241
sjoerdbarts 9:278d25dc0ef3 242 void Set_Mode()
sjoerdbarts 9:278d25dc0ef3 243 {
sjoerdbarts 9:278d25dc0ef3 244 Mode++;
sjoerdbarts 9:278d25dc0ef3 245 if (Mode == 3)
sjoerdbarts 9:278d25dc0ef3 246 {
sjoerdbarts 9:278d25dc0ef3 247 Mode = 0;
sjoerdbarts 9:278d25dc0ef3 248 }
sjoerdbarts 9:278d25dc0ef3 249 pc.printf("\r 0: Kp \r\n");
sjoerdbarts 9:278d25dc0ef3 250 pc.printf("\r 1: Ki \r\n");
sjoerdbarts 9:278d25dc0ef3 251 pc.printf("\r 2: Kd \r\n");
sjoerdbarts 9:278d25dc0ef3 252 pc.printf("\r MODE = %i \r\n",Mode);
sjoerdbarts 9:278d25dc0ef3 253 }
sjoerdbarts 9:278d25dc0ef3 254
sjoerdbarts 9:278d25dc0ef3 255 void increase()
sjoerdbarts 9:278d25dc0ef3 256 {
sjoerdbarts 9:278d25dc0ef3 257 switch(Mode)
sjoerdbarts 9:278d25dc0ef3 258 {
sjoerdbarts 9:278d25dc0ef3 259 case 0:
sjoerdbarts 9:278d25dc0ef3 260 Kp = Kp + 0.01f;
sjoerdbarts 9:278d25dc0ef3 261 break;
sjoerdbarts 9:278d25dc0ef3 262 case 1:
sjoerdbarts 9:278d25dc0ef3 263 Ki = Ki + 0.00001f;
sjoerdbarts 9:278d25dc0ef3 264 break;
sjoerdbarts 9:278d25dc0ef3 265 case 2:
sjoerdbarts 9:278d25dc0ef3 266 Kd = Kd + 0.01f;
sjoerdbarts 9:278d25dc0ef3 267 break;
sjoerdbarts 9:278d25dc0ef3 268 default:
sjoerdbarts 9:278d25dc0ef3 269 pc.printf("\r swtich error \r\n");
sjoerdbarts 9:278d25dc0ef3 270 }
sjoerdbarts 9:278d25dc0ef3 271 pc.printf("\r Kp: %f, Ki: %f, Kd: %f \r\n",Kp,Ki,Kd);
sjoerdbarts 9:278d25dc0ef3 272 }
sjoerdbarts 8:fe907b9a0bab 273
sjoerdbarts 9:278d25dc0ef3 274 void decrease()
sjoerdbarts 9:278d25dc0ef3 275 {
sjoerdbarts 9:278d25dc0ef3 276 switch(Mode)
sjoerdbarts 9:278d25dc0ef3 277 {
sjoerdbarts 9:278d25dc0ef3 278 case 0:
sjoerdbarts 9:278d25dc0ef3 279 if (Kp <= 0.0f)
sjoerdbarts 9:278d25dc0ef3 280 {
sjoerdbarts 9:278d25dc0ef3 281 Kp = 0.0f;
sjoerdbarts 9:278d25dc0ef3 282 break;
sjoerdbarts 9:278d25dc0ef3 283 }
sjoerdbarts 9:278d25dc0ef3 284 Kp = Kp - 0.01f;
sjoerdbarts 9:278d25dc0ef3 285 break;
sjoerdbarts 9:278d25dc0ef3 286 case 1:
sjoerdbarts 9:278d25dc0ef3 287 if (Ki == 0.0f)
sjoerdbarts 9:278d25dc0ef3 288 {
sjoerdbarts 9:278d25dc0ef3 289 Ki = 0.0f;
sjoerdbarts 9:278d25dc0ef3 290 break;
sjoerdbarts 9:278d25dc0ef3 291 }
sjoerdbarts 9:278d25dc0ef3 292 Ki = Ki - 0.00001f;
sjoerdbarts 9:278d25dc0ef3 293 break;
sjoerdbarts 9:278d25dc0ef3 294 case 2:
sjoerdbarts 9:278d25dc0ef3 295 if (Kd == 0.0f)
sjoerdbarts 9:278d25dc0ef3 296 {
sjoerdbarts 9:278d25dc0ef3 297 Kd = 0.0f;
sjoerdbarts 9:278d25dc0ef3 298 break;
sjoerdbarts 9:278d25dc0ef3 299 }
sjoerdbarts 9:278d25dc0ef3 300 Kd = Kd - 0.01f;
sjoerdbarts 9:278d25dc0ef3 301 break;
sjoerdbarts 9:278d25dc0ef3 302 default:
sjoerdbarts 9:278d25dc0ef3 303 pc.printf("\r swtich error \r\n");
sjoerdbarts 9:278d25dc0ef3 304 }
sjoerdbarts 9:278d25dc0ef3 305 pc.printf("\r Kp: %f, Ki: %f, Kd: %f \r\n",Kp,Ki,Kd);
sjoerdbarts 9:278d25dc0ef3 306 }
sjoerdbarts 9:278d25dc0ef3 307
sjoerdbarts 9:278d25dc0ef3 308 double m1_GetPosition()
sjoerdbarts 9:278d25dc0ef3 309 {
sjoerdbarts 9:278d25dc0ef3 310 int countsCW = m1_EncoderCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 311 int countsCCW= m1_EncoderCCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 312 int net_counts=countsCW-countsCCW;
sjoerdbarts 9:278d25dc0ef3 313 double Position=(net_counts*360.0)/COUNTS_PER_REV;
sjoerdbarts 9:278d25dc0ef3 314 return Position;
sjoerdbarts 9:278d25dc0ef3 315 }
sjoerdbarts 6:ed11342ab079 316
sjoerdbarts 9:278d25dc0ef3 317 double m2_GetPosition()
sjoerdbarts 9:278d25dc0ef3 318 {
sjoerdbarts 9:278d25dc0ef3 319 int countsCW = m2_EncoderCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 320 int countsCCW= m2_EncoderCCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 321 int net_counts=countsCW-countsCCW;
sjoerdbarts 9:278d25dc0ef3 322 double Position=(net_counts*360.0)/COUNTS_PER_REV;
sjoerdbarts 9:278d25dc0ef3 323 return Position;
sjoerdbarts 9:278d25dc0ef3 324 }
sjoerdbarts 9:278d25dc0ef3 325
sjoerdbarts 9:278d25dc0ef3 326 double m1_GetPosition_cal()
sjoerdbarts 9:278d25dc0ef3 327 {
sjoerdbarts 9:278d25dc0ef3 328 int countsCW = m1_EncoderCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 329 int countsCCW= m1_EncoderCCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 330 int net_counts=countsCW-countsCCW;
sjoerdbarts 9:278d25dc0ef3 331 double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
sjoerdbarts 9:278d25dc0ef3 332 return Position;
sjoerdbarts 9:278d25dc0ef3 333 }
sjoerdbarts 7:e7aa4f10d1fb 334
sjoerdbarts 9:278d25dc0ef3 335 double m2_GetPosition_cal()
sjoerdbarts 9:278d25dc0ef3 336 {
sjoerdbarts 9:278d25dc0ef3 337 int countsCW = m2_EncoderCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 338 int countsCCW= m2_EncoderCCW.getPulses();
sjoerdbarts 9:278d25dc0ef3 339 int net_counts=countsCW-countsCCW;
sjoerdbarts 9:278d25dc0ef3 340 double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
sjoerdbarts 9:278d25dc0ef3 341 return Position;
sjoerdbarts 9:278d25dc0ef3 342 }
sjoerdbarts 9:278d25dc0ef3 343 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 344 {
sjoerdbarts 9:278d25dc0ef3 345 double a1 = -4/(N*Ts+2),
sjoerdbarts 9:278d25dc0ef3 346 a2 = -1*(N*Ts - 2)/(N*Ts+2),
sjoerdbarts 9:278d25dc0ef3 347 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 348 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
sjoerdbarts 9:278d25dc0ef3 349 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 350
sjoerdbarts 9:278d25dc0ef3 351 double v = error - a1*m1_v1 - a2*m1_v2;
sjoerdbarts 9:278d25dc0ef3 352 double u = b0*v + b1*m1_v1 + b2*m1_v2;
sjoerdbarts 9:278d25dc0ef3 353 m1_v2 = m1_v1; m1_v1 = v;
sjoerdbarts 9:278d25dc0ef3 354 return u;
sjoerdbarts 9:278d25dc0ef3 355 }
sjoerdbarts 9:278d25dc0ef3 356
sjoerdbarts 9:278d25dc0ef3 357 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 358 {
sjoerdbarts 9:278d25dc0ef3 359 double a1 = -4/(N*Ts+2),
sjoerdbarts 9:278d25dc0ef3 360 a2 = -1*(N*Ts - 2)/(N*Ts+2),
sjoerdbarts 9:278d25dc0ef3 361 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 362 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
sjoerdbarts 9:278d25dc0ef3 363 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 364
sjoerdbarts 9:278d25dc0ef3 365 double v = error - a1*m1_v1 - a2*m1_v2;
sjoerdbarts 9:278d25dc0ef3 366 double u = b0*v + b1*m1_v1 + b2*m1_v2;
sjoerdbarts 9:278d25dc0ef3 367 m1_v2 = m1_v1; m1_v1 = v;
sjoerdbarts 9:278d25dc0ef3 368 return u;
sjoerdbarts 9:278d25dc0ef3 369 }
sjoerdbarts 8:fe907b9a0bab 370
sjoerdbarts 8:fe907b9a0bab 371
sjoerdbarts 0:1883abafaa19 372
sjoerdbarts 9:278d25dc0ef3 373 void SetMotor(int motor_number, double MotorValue)
sjoerdbarts 0:1883abafaa19 374 {
sjoerdbarts 8:fe907b9a0bab 375 // Given -1<=motorValue<=1, this sets the PWM and direction
sjoerdbarts 8:fe907b9a0bab 376 // bits for motor 1. Positive value makes motor rotating
sjoerdbarts 8:fe907b9a0bab 377 // clockwise. motorValues outside range are truncated to
sjoerdbarts 8:fe907b9a0bab 378 // within range
sjoerdbarts 9:278d25dc0ef3 379 if (motor_number == 1)
sjoerdbarts 9:278d25dc0ef3 380 {
sjoerdbarts 9:278d25dc0ef3 381 if (MotorValue >=0)
sjoerdbarts 9:278d25dc0ef3 382 {
sjoerdbarts 9:278d25dc0ef3 383 motor1_dir=0;
sjoerdbarts 9:278d25dc0ef3 384 }
sjoerdbarts 9:278d25dc0ef3 385 else
sjoerdbarts 9:278d25dc0ef3 386 {
sjoerdbarts 9:278d25dc0ef3 387 motor1_dir=1;
sjoerdbarts 9:278d25dc0ef3 388 }
sjoerdbarts 9:278d25dc0ef3 389 if (fabs(MotorValue)>1){
sjoerdbarts 9:278d25dc0ef3 390 motor1_pwm.write(1);
sjoerdbarts 9:278d25dc0ef3 391 }
sjoerdbarts 9:278d25dc0ef3 392 else
sjoerdbarts 9:278d25dc0ef3 393 {
sjoerdbarts 9:278d25dc0ef3 394 motor1_pwm.write(abs(MotorValue));
sjoerdbarts 9:278d25dc0ef3 395 }
sjoerdbarts 8:fe907b9a0bab 396 }
sjoerdbarts 9:278d25dc0ef3 397 else
sjoerdbarts 9:278d25dc0ef3 398 {
sjoerdbarts 9:278d25dc0ef3 399 if (MotorValue >=0)
sjoerdbarts 9:278d25dc0ef3 400 {
sjoerdbarts 9:278d25dc0ef3 401 motor2_dir=0;
sjoerdbarts 9:278d25dc0ef3 402 }
sjoerdbarts 9:278d25dc0ef3 403 else
sjoerdbarts 9:278d25dc0ef3 404 {
sjoerdbarts 9:278d25dc0ef3 405 motor2_dir=1;
sjoerdbarts 9:278d25dc0ef3 406 }
sjoerdbarts 9:278d25dc0ef3 407 if (fabs(MotorValue)>1){
sjoerdbarts 9:278d25dc0ef3 408 motor2_pwm.write(1);
sjoerdbarts 9:278d25dc0ef3 409 }
sjoerdbarts 9:278d25dc0ef3 410 else
sjoerdbarts 9:278d25dc0ef3 411 {
sjoerdbarts 9:278d25dc0ef3 412 motor2_pwm.write(abs(MotorValue));
sjoerdbarts 9:278d25dc0ef3 413 }
sjoerdbarts 9:278d25dc0ef3 414 }
sjoerdbarts 8:fe907b9a0bab 415 }
sjoerdbarts 8:fe907b9a0bab 416
sjoerdbarts 8:fe907b9a0bab 417 void BlinkLed(){
sjoerdbarts 8:fe907b9a0bab 418 led = not led;
sjoerdbarts 8:fe907b9a0bab 419 }
sjoerdbarts 8:fe907b9a0bab 420
sjoerdbarts 9:278d25dc0ef3 421 void Controller_motor()
sjoerdbarts 9:278d25dc0ef3 422 {
sjoerdbarts 9:278d25dc0ef3 423 // calculate the reference position
sjoerdbarts 9:278d25dc0ef3 424 CalculationsForTheta();
sjoerdbarts 9:278d25dc0ef3 425 // get the actual position
sjoerdbarts 9:278d25dc0ef3 426 double m1_Position = m1_GetPosition_cal();
sjoerdbarts 9:278d25dc0ef3 427 double m2_Position = m2_GetPosition_cal();
sjoerdbarts 9:278d25dc0ef3 428 // Set position scopes
sjoerdbarts 9:278d25dc0ef3 429 scope.set(0,x);
sjoerdbarts 9:278d25dc0ef3 430 scope.set(1,y);
sjoerdbarts 9:278d25dc0ef3 431 scope.set(2,Theta1);
sjoerdbarts 9:278d25dc0ef3 432 scope.set(3,Theta2);
sjoerdbarts 9:278d25dc0ef3 433 /*
sjoerdbarts 9:278d25dc0ef3 434 scope.set(0,Theta1);
sjoerdbarts 9:278d25dc0ef3 435 scope.set(1,m1_Position);
sjoerdbarts 9:278d25dc0ef3 436 scope.set(3,Theta2);
sjoerdbarts 9:278d25dc0ef3 437 scope.set(4,m2_Position);
sjoerdbarts 9:278d25dc0ef3 438 */
sjoerdbarts 9:278d25dc0ef3 439 //scope.set(2,m1_Position);
sjoerdbarts 9:278d25dc0ef3 440 //scope.set(4,m2_Position);
sjoerdbarts 9:278d25dc0ef3 441 // calc the error
sjoerdbarts 9:278d25dc0ef3 442 double m1_error = Theta1 - m1_Position;
sjoerdbarts 9:278d25dc0ef3 443 double m2_error = Theta2 - m2_Position;
sjoerdbarts 9:278d25dc0ef3 444 //scope.set(2,m1_error);
sjoerdbarts 9:278d25dc0ef3 445 //scope.set(6,m2_error);
sjoerdbarts 9:278d25dc0ef3 446 // calc motorvalues for controller;
sjoerdbarts 9:278d25dc0ef3 447 double m1_MotorValue = m1_PID(m1_error, Kp, Ki, Kd, Ts, N, m1_v1, m1_v2);
sjoerdbarts 9:278d25dc0ef3 448 double m2_MotorValue = m2_PID(m2_error, Kp, Ki, Kd, Ts, N, m2_v1, m2_v2);
sjoerdbarts 9:278d25dc0ef3 449 scope.set(4,m1_MotorValue);
sjoerdbarts 9:278d25dc0ef3 450 scope.set(5,m2_MotorValue);
sjoerdbarts 9:278d25dc0ef3 451 // Set the motorvalues
sjoerdbarts 9:278d25dc0ef3 452 SetMotor(1, m1_MotorValue);
sjoerdbarts 9:278d25dc0ef3 453 SetMotor(2, m2_MotorValue);
sjoerdbarts 9:278d25dc0ef3 454 // Set motorvalues for scope
sjoerdbarts 9:278d25dc0ef3 455 // Send data to HIDScope
sjoerdbarts 9:278d25dc0ef3 456 scope.send();
sjoerdbarts 9:278d25dc0ef3 457 }
sjoerdbarts 9:278d25dc0ef3 458
sjoerdbarts 9:278d25dc0ef3 459 void button1_switch()
sjoerdbarts 9:278d25dc0ef3 460 {
sjoerdbarts 9:278d25dc0ef3 461 button1_value = not button1_value;
sjoerdbarts 9:278d25dc0ef3 462 }
sjoerdbarts 9:278d25dc0ef3 463
sjoerdbarts 9:278d25dc0ef3 464 void PotControl()
sjoerdbarts 9:278d25dc0ef3 465 {
sjoerdbarts 9:278d25dc0ef3 466 double Motor1_Value = (pot1.read() - 0.5f)/2.0f;
sjoerdbarts 9:278d25dc0ef3 467 double Motor2_Value = (pot2.read() - 0.5f)/2.0f;
sjoerdbarts 9:278d25dc0ef3 468 //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
sjoerdbarts 9:278d25dc0ef3 469 //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
sjoerdbarts 9:278d25dc0ef3 470 double m1_Position = m1_GetPosition();
sjoerdbarts 9:278d25dc0ef3 471 double m2_Position = m2_GetPosition();
sjoerdbarts 9:278d25dc0ef3 472
sjoerdbarts 9:278d25dc0ef3 473
sjoerdbarts 9:278d25dc0ef3 474 scope.set(0, Motor1_Value);
sjoerdbarts 9:278d25dc0ef3 475 scope.set(1, m1_Position);
sjoerdbarts 9:278d25dc0ef3 476 scope.set(2, Motor2_Value);
sjoerdbarts 9:278d25dc0ef3 477 scope.set(3, m2_Position);
sjoerdbarts 9:278d25dc0ef3 478 scope.send();
sjoerdbarts 9:278d25dc0ef3 479 // Write the motors
sjoerdbarts 9:278d25dc0ef3 480 SetMotor(1, Motor1_Value);
sjoerdbarts 9:278d25dc0ef3 481 SetMotor(2, Motor2_Value);
sjoerdbarts 9:278d25dc0ef3 482 }
sjoerdbarts 9:278d25dc0ef3 483
sjoerdbarts 9:278d25dc0ef3 484 /*
sjoerdbarts 8:fe907b9a0bab 485 void CalculateSpeed() {
sjoerdbarts 5:36788b154e25 486 countsCW = EncoderCW.getPulses();
sjoerdbarts 5:36788b154e25 487 countsCCW= EncoderCCW.getPulses();
sjoerdbarts 5:36788b154e25 488 net_counts=countsCW-countsCCW;
sjoerdbarts 5:36788b154e25 489 degrees=(net_counts*360.0)/counts_per_rev;
sjoerdbarts 5:36788b154e25 490
sjoerdbarts 8:fe907b9a0bab 491 curr_degrees = degrees;
sjoerdbarts 8:fe907b9a0bab 492 speed = (curr_degrees-prev_degrees)/T_CalculateSpeed;
sjoerdbarts 8:fe907b9a0bab 493 prev_degrees = curr_degrees;
sjoerdbarts 8:fe907b9a0bab 494
sjoerdbarts 8:fe907b9a0bab 495 //scope.set(0, degrees);
sjoerdbarts 8:fe907b9a0bab 496 scope.set(0, speed);
sjoerdbarts 8:fe907b9a0bab 497 double speed_filtered = bqc.step(speed);
sjoerdbarts 8:fe907b9a0bab 498 scope.set(1,speed_filtered);
sjoerdbarts 8:fe907b9a0bab 499 scope.send();
sjoerdbarts 8:fe907b9a0bab 500 }
sjoerdbarts 9:278d25dc0ef3 501 */
sjoerdbarts 8:fe907b9a0bab 502
sjoerdbarts 8:fe907b9a0bab 503 int main(){
sjoerdbarts 8:fe907b9a0bab 504 // Set baud connection with PC
sjoerdbarts 8:fe907b9a0bab 505 pc.baud(SERIAL_BAUD);
sjoerdbarts 8:fe907b9a0bab 506 pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
sjoerdbarts 8:fe907b9a0bab 507
sjoerdbarts 8:fe907b9a0bab 508 // Setup Blinking LED
sjoerdbarts 8:fe907b9a0bab 509 led = 1;
sjoerdbarts 8:fe907b9a0bab 510 Ticker TickerBlinkLed;
sjoerdbarts 8:fe907b9a0bab 511 TickerBlinkLed.attach(BlinkLed,0.5);
sjoerdbarts 8:fe907b9a0bab 512
sjoerdbarts 8:fe907b9a0bab 513 // Set motor PWM speeds
sjoerdbarts 8:fe907b9a0bab 514 motor1_pwm.period(1.0/1000);
sjoerdbarts 9:278d25dc0ef3 515 motor2_pwm.period(1.0/1000);
sjoerdbarts 8:fe907b9a0bab 516
sjoerdbarts 9:278d25dc0ef3 517 // Setup Interruptin.fall
sjoerdbarts 9:278d25dc0ef3 518 button1.fall(button1_switch);
sjoerdbarts 9:278d25dc0ef3 519 button2.fall(Set_Mode);
sjoerdbarts 9:278d25dc0ef3 520 button3.fall(increase);
sjoerdbarts 9:278d25dc0ef3 521 button4.fall(decrease);
sjoerdbarts 9:278d25dc0ef3 522 // Setup motor control
sjoerdbarts 9:278d25dc0ef3 523 Ticker PIDControllerTicker;
sjoerdbarts 9:278d25dc0ef3 524 Ticker PotControlTicker;
sjoerdbarts 9:278d25dc0ef3 525
sjoerdbarts 9:278d25dc0ef3 526 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 527 pc.printf("\r\n Press button SW3 to start positioning the arms using PotControl\r\n");
sjoerdbarts 9:278d25dc0ef3 528 pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
sjoerdbarts 9:278d25dc0ef3 529 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 530 while (button1_value == false){}
sjoerdbarts 9:278d25dc0ef3 531 PotControlTicker.attach(&PotControl,0.01f);
sjoerdbarts 8:fe907b9a0bab 532
sjoerdbarts 9:278d25dc0ef3 533 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 534 pc.printf("\r\n When done positioning, press button SW3 to detach Potcontrol");
sjoerdbarts 9:278d25dc0ef3 535 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 536 while (button1_value == true){}
sjoerdbarts 9:278d25dc0ef3 537
sjoerdbarts 9:278d25dc0ef3 538 // Detach potcontrol
sjoerdbarts 9:278d25dc0ef3 539 PotControlTicker.detach();
sjoerdbarts 9:278d25dc0ef3 540
sjoerdbarts 9:278d25dc0ef3 541 // Set motors to 0
sjoerdbarts 9:278d25dc0ef3 542 motor2_pwm.write(0);
sjoerdbarts 9:278d25dc0ef3 543 motor2_pwm.write(0);
sjoerdbarts 8:fe907b9a0bab 544
sjoerdbarts 9:278d25dc0ef3 545 // Reset the encoders to set the 0 position
sjoerdbarts 9:278d25dc0ef3 546 m1_EncoderCW.reset();
sjoerdbarts 9:278d25dc0ef3 547 m1_EncoderCCW.reset();
sjoerdbarts 9:278d25dc0ef3 548 m2_EncoderCW.reset();
sjoerdbarts 9:278d25dc0ef3 549 m2_EncoderCCW.reset();
sjoerdbarts 9:278d25dc0ef3 550
sjoerdbarts 9:278d25dc0ef3 551 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 552 pc.printf("\r\n When done positioning, press button SW3 to start potmeter PID control");
sjoerdbarts 9:278d25dc0ef3 553 pc.printf("\r\n Make sure both potentiometers are positioned halfway!!! \r\n");
sjoerdbarts 9:278d25dc0ef3 554 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 9:278d25dc0ef3 555 while (button1_value == false){}
sjoerdbarts 9:278d25dc0ef3 556 PIDControllerTicker.attach(&Controller_motor, CONTROLLER_TS); // 100 Hz
sjoerdbarts 9:278d25dc0ef3 557 while(true){}
sjoerdbarts 0:1883abafaa19 558 }