Biorobotics_group_2 / Mbed 2 deprecated CompleteFunction

Alle subfiles samengevoegd

Dependencies:   HIDScope biquadFilter mbed MODSERIAL FastPWM QEI

main.cpp@8:676646c50aeb, 2016-10-31 (annotated)

Committer:
sjoerdbarts
Date:
Mon Oct 31 12:38:14 2016 +0000
Revision:
8:676646c50aeb
Parent:
7:bafc32b576c4
Child:
9:6cee14e0e323
Initial final code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1600907 0:c96cf9760185 1 #include "mbed.h"
s1600907 0:c96cf9760185 2 #include "math.h"
s1600907 1:e1267e72ade8 3 #include "MODSERIAL.h"
sjoerdbarts 6:00011220b82b 4 #include "HIDScope.h"
sjoerdbarts 6:00011220b82b 5 #include "FastPWM.h"
sjoerdbarts 6:00011220b82b 6 #include "QEI.h"
sjoerdbarts 6:00011220b82b 7 #include "BiQuad.h"
sjoerdbarts 6:00011220b82b 8 #define SERIAL_BAUD 115200 // baud rate for serial communication
s1600907 0:c96cf9760185 9
sjoerdbarts 6:00011220b82b 10 // Serial connection
s1600907 1:e1267e72ade8 11 MODSERIAL pc(USBTX,USBRX);
sjoerdbarts 6:00011220b82b 12
s1600907 0:c96cf9760185 13 //EMG aansluitingen
s1600907 0:c96cf9760185 14 AnalogIn emg0( A0 ); //Biceps Rechts
s1600907 2:92593c9d6146 15 AnalogIn emg1( A1 ); //Bicpes Links
s1600907 2:92593c9d6146 16 AnalogIn emg2( A2 ); //Upper leg
sjoerdbarts 6:00011220b82b 17
sjoerdbarts 6:00011220b82b 18 // Potmeter 1 gives the reference position x
sjoerdbarts 6:00011220b82b 19 AnalogIn pot1(A3);
sjoerdbarts 6:00011220b82b 20 // Potmeter 2 gives the reference position y
sjoerdbarts 6:00011220b82b 21 AnalogIn pot2(A4);
sjoerdbarts 6:00011220b82b 22
sjoerdbarts 6:00011220b82b 23 // Setup Buttons
sjoerdbarts 6:00011220b82b 24 InterruptIn button1(PTB9); // button 1
sjoerdbarts 6:00011220b82b 25 InterruptIn button2(PTA1); // button 2
sjoerdbarts 6:00011220b82b 26 InterruptIn button3(PTC6); // SW2
sjoerdbarts 6:00011220b82b 27 InterruptIn button4(PTA4); // SW3
sjoerdbarts 6:00011220b82b 28
sjoerdbarts 8:676646c50aeb 29 // Setup botton values
sjoerdbarts 8:676646c50aeb 30 bool button1_value = false;
sjoerdbarts 8:676646c50aeb 31 bool button2_value = false;
sjoerdbarts 8:676646c50aeb 32
sjoerdbarts 6:00011220b82b 33 // Set motor Pinouts
sjoerdbarts 6:00011220b82b 34 DigitalOut motor1_dir(D4);
sjoerdbarts 6:00011220b82b 35 FastPWM motor1_pwm(D5);
sjoerdbarts 6:00011220b82b 36 DigitalOut motor2_dir(D7);
sjoerdbarts 6:00011220b82b 37 FastPWM motor2_pwm(D6);
sjoerdbarts 6:00011220b82b 38
sjoerdbarts 6:00011220b82b 39 // Set LED pins
sjoerdbarts 6:00011220b82b 40 DigitalOut led(LED_RED);
sjoerdbarts 6:00011220b82b 41
sjoerdbarts 6:00011220b82b 42 // Set HID scope
sjoerdbarts 6:00011220b82b 43 HIDScope scope(6);
sjoerdbarts 6:00011220b82b 44
sjoerdbarts 6:00011220b82b 45 // Set encoder
sjoerdbarts 6:00011220b82b 46 QEI m1_EncoderCW(D10,D11,NC,32);
sjoerdbarts 6:00011220b82b 47 QEI m1_EncoderCCW(D11,D10,NC,32);
sjoerdbarts 6:00011220b82b 48 QEI m2_EncoderCW(D13,D12,NC,32);
sjoerdbarts 6:00011220b82b 49 QEI m2_EncoderCCW(D12,D13,NC,32);
s1600907 0:c96cf9760185 50
sjoerdbarts 7:bafc32b576c4 51 // Constants for control
sjoerdbarts 7:bafc32b576c4 52 volatile const int COUNTS_PER_REV = 8400;
sjoerdbarts 8:676646c50aeb 53 volatile const double DEGREES_PER_PULSE = 8400 / 360;
sjoerdbarts 7:bafc32b576c4 54
sjoerdbarts 7:bafc32b576c4 55 // Set initial Kp and Ki
sjoerdbarts 7:bafc32b576c4 56 volatile double Kp = 0.01; // last known good Kp: 0.01
sjoerdbarts 8:676646c50aeb 57 volatile double Ki = 0.0025; // last known good Ki: 0.0025
sjoerdbarts 7:bafc32b576c4 58 volatile double Kd = 0.0; // last known good Kd: 0.0
sjoerdbarts 7:bafc32b576c4 59
sjoerdbarts 8:676646c50aeb 60 // Set frequencies for sampling, calc and control.
sjoerdbarts 8:676646c50aeb 61 volatile const double SAMPLE_F = 500; // 500 Hz
sjoerdbarts 8:676646c50aeb 62 volatile const double SAMPLE_TS = 1.0/SAMPLE_F;
sjoerdbarts 8:676646c50aeb 63 volatile const double CALC_F = 100.0; // 100 Hz
sjoerdbarts 8:676646c50aeb 64 volatile const double CALC_TS = 1.0/CALC_F;
sjoerdbarts 8:676646c50aeb 65 volatile const double CONTROLLER_TS = 0.01; // 100 Hz
sjoerdbarts 8:676646c50aeb 66
sjoerdbarts 7:bafc32b576c4 67 volatile double Ts = 0.01;
sjoerdbarts 7:bafc32b576c4 68 volatile double N = 100;
sjoerdbarts 7:bafc32b576c4 69
sjoerdbarts 7:bafc32b576c4 70 // Memory values for controllers
sjoerdbarts 7:bafc32b576c4 71 double m1_v1 = 0, m1_v2 = 0;
sjoerdbarts 7:bafc32b576c4 72 double m2_v1 = 0, m2_v2 = 0;
s1600907 0:c96cf9760185 73
s1600907 0:c96cf9760185 74 //BiQuad
s1600907 0:c96cf9760185 75 // making the biquads and chains; imported from matlab
s1600907 0:c96cf9760185 76 BiQuadChain bqc; //Chain for right biceps
s1600907 0:c96cf9760185 77 BiQuadChain bqc2; //Chain for left biceps
s1600907 0:c96cf9760185 78 BiQuadChain bqc3; //Chain for Upper leg
s1600907 0:c96cf9760185 79 // 1 to 3 are for right biceps
s1600907 0:c96cf9760185 80 BiQuad bq1( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 81 BiQuad bq2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 82 BiQuad bq3( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
s1600907 0:c96cf9760185 83 // 4 to 6 are for left biceps
s1600907 0:c96cf9760185 84 BiQuad bq4( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 85 BiQuad bq5( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 86 BiQuad bq6( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
s1600907 0:c96cf9760185 87 // 7 to 9 are for upper leg
s1600907 0:c96cf9760185 88 BiQuad bq7( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 89 BiQuad bq8( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 90 BiQuad bq9( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
s1600907 0:c96cf9760185 91
sjoerdbarts 7:bafc32b576c4 92 // Variables for filter
sjoerdbarts 7:bafc32b576c4 93 // muscle EMG < treshhold
s1600907 0:c96cf9760185 94 volatile bool BicepsLeft = false;
s1600907 0:c96cf9760185 95 volatile bool BicepsRight = false;
s1600907 0:c96cf9760185 96 volatile bool Leg = false;
s1600907 0:c96cf9760185 97
sjoerdbarts 7:bafc32b576c4 98 // Variables for MotorHoekBerekenen (MotorAngleCalculation)
s1600907 1:e1267e72ade8 99 volatile double x = 0.0; // beginpositie x en y
s1600907 1:e1267e72ade8 100 volatile double y = 305.5;
s1600907 0:c96cf9760185 101 volatile const double pi = 3.14159265359;
s1600907 3:c524afedf863 102 volatile double Theta1Gear = 60.0; // Beginpositie op 60 graden
s1600907 3:c524afedf863 103 volatile double Theta2Gear = 60.0;
s1600907 1:e1267e72ade8 104 volatile double Theta1 = Theta1Gear*42/12; // Beginpositie van MotorHoek
s1600907 1:e1267e72ade8 105 volatile double Theta2 = Theta2Gear*42/12;
sjoerdbarts 8:676646c50aeb 106
s1600907 0:c96cf9760185 107 bool Calibration = true; // beginnen met calibreren
s1600907 0:c96cf9760185 108 volatile bool Stepper_State = false; // false = we zijn niet bezig met flippen
sjoerdbarts 8:676646c50aeb 109 volatile double Speed = 10; // mm/s the position is changed
s1600907 0:c96cf9760185 110
sjoerdbarts 7:bafc32b576c4 111 /////////////////////// functions for filters /////////////////////////////////////////////
s1600907 0:c96cf9760185 112 void sample()
s1600907 0:c96cf9760185 113 {
s1600907 0:c96cf9760185 114 /* Sample function
s1600907 0:c96cf9760185 115 samples the emg0 (of the rightBiceps)
s1600907 0:c96cf9760185 116 samples the emg1 (of the leftBiceps)
s1600907 0:c96cf9760185 117 samples the emg2 (of the Upper leg)
s1600907 0:c96cf9760185 118 filter the singals
s1600907 0:c96cf9760185 119 sends it to HIDScope
s1600907 0:c96cf9760185 120 */
s1600907 0:c96cf9760185 121
s1600907 0:c96cf9760185 122 // Rechts Biceps
s1600907 0:c96cf9760185 123 double inRechts = emg0.read(); // EMG signal
s1600907 0:c96cf9760185 124 double FilterRechts = bqc.step(inRechts); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 125 double RectifyRechts = fabs(FilterRechts); // Rectify
s1600907 0:c96cf9760185 126 double outRechts = bq3.step(RectifyRechts); // Low pass
s1600907 2:92593c9d6146 127
s1600907 0:c96cf9760185 128 // Links Biceps
s1600907 0:c96cf9760185 129 double inLinks = emg1.read(); // EMG signal
s1600907 0:c96cf9760185 130 double FilterLinks = bqc2.step(inLinks); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 131 double RectifyLinks = fabs(FilterLinks); // Rectify
s1600907 0:c96cf9760185 132 double outLinks = bq6.step(RectifyLinks); // Low pass
s1600907 0:c96cf9760185 133
s1600907 0:c96cf9760185 134 // Upper leg
s1600907 0:c96cf9760185 135 double inBeen = emg2.read(); // EMG signal
s1600907 0:c96cf9760185 136 double FilterBeen = bqc3.step(inBeen); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 137 double RectifyBeen = fabs(FilterBeen); // Rectify
s1600907 2:92593c9d6146 138 double outBeen = bq9.step(RectifyBeen); // Low pass
s1600907 0:c96cf9760185 139
s1600907 0:c96cf9760185 140
s1600907 0:c96cf9760185 141 /* EMG signal in channel 0 (the first channel)
s1600907 0:c96cf9760185 142 and the filtered EMG signal in channel 1 (the second channel)
s1600907 0:c96cf9760185 143 of the HIDscope */
s1600907 5:b4a0301ec8cc 144 /*scope.set(0,inRechts);
s1600907 0:c96cf9760185 145 scope.set(1,outRechts);
s1600907 2:92593c9d6146 146 scope.set(2,inLinks);
s1600907 0:c96cf9760185 147 scope.set(3,outLinks);
s1600907 0:c96cf9760185 148 scope.set(4,inBeen);
s1600907 0:c96cf9760185 149 scope.set(5,outBeen);
s1600907 5:b4a0301ec8cc 150 */
s1600907 0:c96cf9760185 151 // To indicate that the function is working, the LED is on
s1600907 1:e1267e72ade8 152 if (outRechts >= 0.04){
s1600907 0:c96cf9760185 153 BicepsRight = true;
s1600907 0:c96cf9760185 154 }
s1600907 0:c96cf9760185 155 else{
s1600907 0:c96cf9760185 156 BicepsRight = false;
s1600907 0:c96cf9760185 157 }
s1600907 2:92593c9d6146 158
s1600907 0:c96cf9760185 159 // If Biceps links is actuated then:
s1600907 1:e1267e72ade8 160 if (outLinks >= 0.04){
s1600907 0:c96cf9760185 161 BicepsLeft = true;
s1600907 0:c96cf9760185 162 }
s1600907 0:c96cf9760185 163 else{
s1600907 0:c96cf9760185 164 BicepsLeft = false;
s1600907 0:c96cf9760185 165 }
s1600907 0:c96cf9760185 166 // If upper leg is actuated then:
s1600907 0:c96cf9760185 167 if (outBeen >= 0.01){
s1600907 0:c96cf9760185 168 Leg = true;
s1600907 0:c96cf9760185 169 }
s1600907 0:c96cf9760185 170 else{
s1600907 0:c96cf9760185 171 Leg = false;
s1600907 0:c96cf9760185 172 }
s1600907 0:c96cf9760185 173 }
s1600907 0:c96cf9760185 174
sjoerdbarts 7:bafc32b576c4 175 ///////////////////// functions for "motorhoekberekenen" //////////////////////////
sjoerdbarts 7:bafc32b576c4 176 // save previous x
s1600907 0:c96cf9760185 177 double Calc_Prev_x () {
s1600907 0:c96cf9760185 178 double Prev_x = x;
s1600907 0:c96cf9760185 179 //pc.printf("prev x = %f\r\n", Prev_x);
s1600907 0:c96cf9760185 180 return Prev_x;
s1600907 0:c96cf9760185 181 }
s1600907 0:c96cf9760185 182
sjoerdbarts 7:bafc32b576c4 183 // save previous y
s1600907 0:c96cf9760185 184 double Calc_Prev_y () {
s1600907 0:c96cf9760185 185 double Prev_y = y;
s1600907 0:c96cf9760185 186 //pc.printf("prev y = %f\r\n", Prev_y);
s1600907 0:c96cf9760185 187 return Prev_y;
s1600907 0:c96cf9760185 188 }
s1600907 0:c96cf9760185 189
sjoerdbarts 7:bafc32b576c4 190 // save previous Theta1Gear
s1600907 4:05c5da1c8b08 191 double Calc_Prev_Theta1_Gear () {
s1600907 4:05c5da1c8b08 192 double Prev_Theta1_Gear = Theta1Gear;
s1600907 4:05c5da1c8b08 193 return Prev_Theta1_Gear;
s1600907 4:05c5da1c8b08 194 }
s1600907 4:05c5da1c8b08 195
sjoerdbarts 7:bafc32b576c4 196 // save previous Theta2Gear
s1600907 4:05c5da1c8b08 197 double Calc_Prev_Theta2_Gear () {
s1600907 4:05c5da1c8b08 198 double Prev_Theta2_Gear = Theta2Gear;
s1600907 4:05c5da1c8b08 199 return Prev_Theta2_Gear;
s1600907 4:05c5da1c8b08 200 }
s1600907 4:05c5da1c8b08 201
s1600907 2:92593c9d6146 202 void FunctieFlipper(); // declarate function, function discribed below
s1600907 0:c96cf9760185 203 // berekenen van de nieuwe x en y waardes
sjoerdbarts 8:676646c50aeb 204 void CalcXY ()
sjoerdbarts 7:bafc32b576c4 205 {
sjoerdbarts 8:676646c50aeb 206 // calc steps in mm
sjoerdbarts 8:676646c50aeb 207 double Step = Speed/CALC_F ; //10 mm per seconde afleggen
s1600907 0:c96cf9760185 208
s1600907 0:c96cf9760185 209 if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) {
s1600907 0:c96cf9760185 210 Stepper_State = true; // we zijn aan het flipper dus geen andere dingen doen
s1600907 2:92593c9d6146 211 FunctieFlipper() ;
s1600907 0:c96cf9760185 212 }
s1600907 0:c96cf9760185 213 else if (BicepsLeft==true && BicepsRight==false && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 214 x = x - Step;
s1600907 0:c96cf9760185 215 }
s1600907 0:c96cf9760185 216 else if (BicepsLeft==false && BicepsRight==true && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 217 x = x + Step; // naar Right bewegen
s1600907 0:c96cf9760185 218 }
s1600907 0:c96cf9760185 219 else if (BicepsLeft==true && BicepsRight==true && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 220 y = y + Step; // naar voren bewegen
s1600907 0:c96cf9760185 221 }
s1600907 0:c96cf9760185 222 else if (BicepsLeft==false && BicepsRight==true && Leg==true && Stepper_State==false) {
s1600907 0:c96cf9760185 223 y = y - Step; // naar achter bewegen
s1600907 0:c96cf9760185 224 }
s1600907 0:c96cf9760185 225 else if (BicepsLeft==false && BicepsRight==false && Leg==false || Stepper_State==true) {
s1600907 0:c96cf9760185 226 }
s1600907 0:c96cf9760185 227
s1600907 0:c96cf9760185 228 // Grenswaardes LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
s1600907 0:c96cf9760185 229 if (x > 200) {
s1600907 0:c96cf9760185 230 x = 200;
s1600907 0:c96cf9760185 231 }
s1600907 0:c96cf9760185 232 else if (x < -200) {
s1600907 0:c96cf9760185 233 x = -200;
s1600907 0:c96cf9760185 234 }
s1600907 0:c96cf9760185 235 if (y > 306) {
s1600907 0:c96cf9760185 236 y = 306;
s1600907 0:c96cf9760185 237 }
s1600907 0:c96cf9760185 238 else if (y < 50) {
s1600907 0:c96cf9760185 239 y = 50; // GOKJE, UITPROBEREN
s1600907 0:c96cf9760185 240 }
s1600907 5:b4a0301ec8cc 241 //pc.printf("x = %f, y = %f\r\n", x, y);
s1600907 1:e1267e72ade8 242
s1600907 2:92593c9d6146 243 //scope.set(2,x);
s1600907 2:92593c9d6146 244 //scope.set(3,y);
s1600907 0:c96cf9760185 245 }
s1600907 0:c96cf9760185 246
s1600907 0:c96cf9760185 247 // diagonaal berekenen voor linker arm
s1600907 0:c96cf9760185 248 double CalcDia1 (double x, double y) {
s1600907 0:c96cf9760185 249 double a = 50.0; // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden) KEERTJE NAMETEN
s1600907 0:c96cf9760185 250 double BV1 = sqrt(pow((a+x),2) + pow(y,2)); // diagonaal (afstand van armas tot locatie) berekenen
s1600907 0:c96cf9760185 251 double Dia1 = pow(BV1,2)/(2*BV1); // berekenen van de afstand oorsprong tot diagonaal
s1600907 0:c96cf9760185 252
s1600907 0:c96cf9760185 253 //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia1, x, y);
s1600907 0:c96cf9760185 254 return Dia1;
s1600907 0:c96cf9760185 255 }
s1600907 0:c96cf9760185 256
s1600907 0:c96cf9760185 257 // diagonaal berekenen voor rechter arm
s1600907 0:c96cf9760185 258 double CalcDia2 (double x, double y) {
s1600907 0:c96cf9760185 259 double a = 50.0;
s1600907 0:c96cf9760185 260 double BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm
s1600907 0:c96cf9760185 261 double Dia2 = pow(BV2,2)/(2*BV2);
s1600907 0:c96cf9760185 262
s1600907 0:c96cf9760185 263 //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia2, x, y);
s1600907 0:c96cf9760185 264 return Dia2;
s1600907 0:c96cf9760185 265 }
s1600907 0:c96cf9760185 266
s1600907 0:c96cf9760185 267 // calculate Theta1
s1600907 4:05c5da1c8b08 268 void CalcTheta1 (double Dia1) {
s1600907 0:c96cf9760185 269 double a = 50.0;
s1600907 0:c96cf9760185 270 double Bar = 200.0; // lengte van de armen
s1600907 0:c96cf9760185 271
s1600907 0:c96cf9760185 272 // Hoek berekenen van het grote tandwiel (gear)
s1600907 0:c96cf9760185 273 if (x > -a) {
s1600907 0:c96cf9760185 274 Theta1Gear = pi - atan(y/(x+a)) - acos(Dia1/Bar);
s1600907 0:c96cf9760185 275 }
s1600907 0:c96cf9760185 276 else if (x > -a) {
s1600907 0:c96cf9760185 277 Theta1Gear = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
s1600907 0:c96cf9760185 278 }
s1600907 0:c96cf9760185 279 else { // als x=-a
s1600907 0:c96cf9760185 280 Theta1Gear = 0.5*pi - acos(Dia1/Bar);
s1600907 0:c96cf9760185 281 }
s1600907 3:c524afedf863 282 Theta1Gear = Theta1Gear*180.0/pi; // veranderen van radialen naar graden
s1600907 0:c96cf9760185 283
s1600907 0:c96cf9760185 284 // omrekenen van grote tandwiel naar motortandwiel
s1600907 0:c96cf9760185 285 Theta1 = Theta1Gear*42.0/12.0; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
s1600907 0:c96cf9760185 286
s1600907 5:b4a0301ec8cc 287 pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta1, Theta1Gear);
s1600907 0:c96cf9760185 288 }
s1600907 0:c96cf9760185 289
s1600907 4:05c5da1c8b08 290 void CalcTheta2 (double Dia2) {
s1600907 0:c96cf9760185 291 double a = 50.0;
s1600907 0:c96cf9760185 292 double Bar = 200.0; // lengte van de armen
s1600907 0:c96cf9760185 293 double Prev_Theta2_Gear = Theta2Gear;
s1600907 4:05c5da1c8b08 294
s1600907 0:c96cf9760185 295 // Hoek berekenen van het grote tandwiel (gear)
s1600907 0:c96cf9760185 296 if (x < a) {
s1600907 0:c96cf9760185 297 Theta2Gear = pi + atan(y/(x-a)) - acos(Dia2/Bar);
s1600907 0:c96cf9760185 298 }
s1600907 0:c96cf9760185 299 else if (x > a) {
s1600907 0:c96cf9760185 300 Theta2Gear = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
s1600907 0:c96cf9760185 301 }
s1600907 0:c96cf9760185 302 else { // als x=a
s1600907 0:c96cf9760185 303 Theta2Gear = 0.5*pi - acos(Dia2/Bar);
s1600907 0:c96cf9760185 304 }
s1600907 3:c524afedf863 305 Theta2Gear = Theta2Gear*180/pi; // veranderen van radialen naar graden
s1600907 0:c96cf9760185 306
s1600907 4:05c5da1c8b08 307 // omrekenen van grote tandwiel naar motortandwiel
s1600907 4:05c5da1c8b08 308 Theta2 = Theta2Gear*42.0/12.0; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
s1600907 4:05c5da1c8b08 309
s1600907 5:b4a0301ec8cc 310 pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
s1600907 4:05c5da1c8b08 311 }
s1600907 4:05c5da1c8b08 312
s1600907 4:05c5da1c8b08 313 // als een van de hoeken te groot wordt, zet dan alles terug naar de vorige positie
s1600907 4:05c5da1c8b08 314 void AngleLimits (double Prev_Theta1_Gear, double Prev_Theta2_Gear, double Prev_x, double Prev_y) {
s1600907 4:05c5da1c8b08 315 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)
s1600907 4:05c5da1c8b08 316
s1600907 4:05c5da1c8b08 317 if (Theta1Gear >= MaxThetaGear || Theta2Gear >= MaxThetaGear) {
s1600907 4:05c5da1c8b08 318 Theta1Gear = Prev_Theta1_Gear;
s1600907 0:c96cf9760185 319 Theta2Gear = Prev_Theta2_Gear;
s1600907 0:c96cf9760185 320 x = Prev_x;
s1600907 0:c96cf9760185 321 y = Prev_y;
s1600907 4:05c5da1c8b08 322
s1600907 4:05c5da1c8b08 323 Theta1 = Theta1Gear*42.0/12.0;
s1600907 4:05c5da1c8b08 324 Theta2 = Theta2Gear*42.0/12.0;
s1600907 0:c96cf9760185 325 }
s1600907 5:b4a0301ec8cc 326 pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
s1600907 0:c96cf9760185 327 }
s1600907 0:c96cf9760185 328
s1600907 0:c96cf9760185 329 void CalculationsForTheta () {
s1600907 0:c96cf9760185 330 double Prev_x = Calc_Prev_x ();
s1600907 0:c96cf9760185 331 double Prev_y = Calc_Prev_y ();
sjoerdbarts 8:676646c50aeb 332 double Prev_Theta1_Gear = Calc_Prev_Theta1_Gear ();
sjoerdbarts 8:676646c50aeb 333 double Prev_Theta2_Gear = Calc_Prev_Theta2_Gear ();
sjoerdbarts 8:676646c50aeb 334 CalcXY();
s1600907 0:c96cf9760185 335 double Dia1 = CalcDia1 (x, y);
s1600907 0:c96cf9760185 336 double Dia2 = CalcDia2 (x, y);
s1600907 4:05c5da1c8b08 337 CalcTheta1 (Dia1);
sjoerdbarts 8:676646c50aeb 338 CalcTheta2 (Dia2);
sjoerdbarts 8:676646c50aeb 339 AngleLimits (Prev_Theta1_Gear, Prev_Theta2_Gear, Prev_x, Prev_y); // laatste check
s1600907 0:c96cf9760185 340 }
s1600907 0:c96cf9760185 341
sjoerdbarts 8:676646c50aeb 342 ////////////////////////// Code for control /////////////////////////////////////
sjoerdbarts 8:676646c50aeb 343
sjoerdbarts 8:676646c50aeb 344 double m1_GetPosition()
sjoerdbarts 8:676646c50aeb 345 {
sjoerdbarts 8:676646c50aeb 346 int countsCW = m1_EncoderCW.getPulses();
sjoerdbarts 8:676646c50aeb 347 int countsCCW= m1_EncoderCCW.getPulses();
sjoerdbarts 8:676646c50aeb 348 int net_counts=countsCW-countsCCW;
sjoerdbarts 8:676646c50aeb 349 double Position=(net_counts*360.0)/COUNTS_PER_REV;
sjoerdbarts 8:676646c50aeb 350 return Position;
sjoerdbarts 8:676646c50aeb 351 }
sjoerdbarts 8:676646c50aeb 352
sjoerdbarts 8:676646c50aeb 353 double m2_GetPosition()
sjoerdbarts 8:676646c50aeb 354 {
sjoerdbarts 8:676646c50aeb 355 int countsCW = m2_EncoderCW.getPulses();
sjoerdbarts 8:676646c50aeb 356 int countsCCW= m2_EncoderCCW.getPulses();
sjoerdbarts 8:676646c50aeb 357 int net_counts=countsCW-countsCCW;
sjoerdbarts 8:676646c50aeb 358 double Position=(net_counts*360.0)/COUNTS_PER_REV;
sjoerdbarts 8:676646c50aeb 359 return Position;
sjoerdbarts 8:676646c50aeb 360 }
sjoerdbarts 8:676646c50aeb 361
sjoerdbarts 8:676646c50aeb 362 // Position control when calibrating
sjoerdbarts 8:676646c50aeb 363 double m1_GetPosition_cal()
sjoerdbarts 8:676646c50aeb 364 {
sjoerdbarts 8:676646c50aeb 365 int countsCW = m1_EncoderCW.getPulses();
sjoerdbarts 8:676646c50aeb 366 int countsCCW= m1_EncoderCCW.getPulses();
sjoerdbarts 8:676646c50aeb 367 int net_counts=countsCW-countsCCW;
sjoerdbarts 8:676646c50aeb 368 double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
sjoerdbarts 8:676646c50aeb 369 return Position;
sjoerdbarts 8:676646c50aeb 370 }
sjoerdbarts 8:676646c50aeb 371
sjoerdbarts 8:676646c50aeb 372 // Position control when calibrating
sjoerdbarts 8:676646c50aeb 373 double m2_GetPosition_cal()
sjoerdbarts 8:676646c50aeb 374 {
sjoerdbarts 8:676646c50aeb 375 int countsCW = m2_EncoderCW.getPulses();
sjoerdbarts 8:676646c50aeb 376 int countsCCW= m2_EncoderCCW.getPulses();
sjoerdbarts 8:676646c50aeb 377 int net_counts=countsCW-countsCCW;
sjoerdbarts 8:676646c50aeb 378 double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
sjoerdbarts 8:676646c50aeb 379 return Position;
sjoerdbarts 8:676646c50aeb 380 }
sjoerdbarts 8:676646c50aeb 381 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 8:676646c50aeb 382 {
sjoerdbarts 8:676646c50aeb 383 double a1 = -4/(N*Ts+2),
sjoerdbarts 8:676646c50aeb 384 a2 = -1*(N*Ts - 2)/(N*Ts+2),
sjoerdbarts 8:676646c50aeb 385 b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
sjoerdbarts 8:676646c50aeb 386 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
sjoerdbarts 8:676646c50aeb 387 b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
sjoerdbarts 8:676646c50aeb 388
sjoerdbarts 8:676646c50aeb 389 double v = error - a1*m1_v1 - a2*m1_v2;
sjoerdbarts 8:676646c50aeb 390 double u = b0*v + b1*m1_v1 + b2*m1_v2;
sjoerdbarts 8:676646c50aeb 391 m1_v2 = m1_v1; m1_v1 = v;
sjoerdbarts 8:676646c50aeb 392 return u;
sjoerdbarts 8:676646c50aeb 393 }
sjoerdbarts 8:676646c50aeb 394
sjoerdbarts 8:676646c50aeb 395 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 8:676646c50aeb 396 {
sjoerdbarts 8:676646c50aeb 397 double a1 = -4/(N*Ts+2),
sjoerdbarts 8:676646c50aeb 398 a2 = -1*(N*Ts - 2)/(N*Ts+2),
sjoerdbarts 8:676646c50aeb 399 b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
sjoerdbarts 8:676646c50aeb 400 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
sjoerdbarts 8:676646c50aeb 401 b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
sjoerdbarts 8:676646c50aeb 402
sjoerdbarts 8:676646c50aeb 403 double v = error - a1*m1_v1 - a2*m1_v2;
sjoerdbarts 8:676646c50aeb 404 double u = b0*v + b1*m1_v1 + b2*m1_v2;
sjoerdbarts 8:676646c50aeb 405 m1_v2 = m1_v1; m1_v1 = v;
sjoerdbarts 8:676646c50aeb 406 return u;
sjoerdbarts 8:676646c50aeb 407 }
sjoerdbarts 8:676646c50aeb 408
sjoerdbarts 8:676646c50aeb 409
s1600907 0:c96cf9760185 410
sjoerdbarts 8:676646c50aeb 411 void SetMotor(int motor_number, double MotorValue)
sjoerdbarts 8:676646c50aeb 412 {
sjoerdbarts 8:676646c50aeb 413 // Given -1<=motorValue<=1, this sets the PWM and direction
sjoerdbarts 8:676646c50aeb 414 // bits for motor 1. Positive value makes motor rotating
sjoerdbarts 8:676646c50aeb 415 // clockwise. motorValues outside range are truncated to
sjoerdbarts 8:676646c50aeb 416 // within range
sjoerdbarts 8:676646c50aeb 417 if (motor_number == 1)
sjoerdbarts 8:676646c50aeb 418 {
sjoerdbarts 8:676646c50aeb 419 if (MotorValue >=0)
sjoerdbarts 8:676646c50aeb 420 {
sjoerdbarts 8:676646c50aeb 421 motor1_dir=0;
sjoerdbarts 8:676646c50aeb 422 }
sjoerdbarts 8:676646c50aeb 423 else
sjoerdbarts 8:676646c50aeb 424 {
sjoerdbarts 8:676646c50aeb 425 motor1_dir=1;
sjoerdbarts 8:676646c50aeb 426 }
sjoerdbarts 8:676646c50aeb 427 if (fabs(MotorValue)>1){
sjoerdbarts 8:676646c50aeb 428 motor1_pwm.write(1);
sjoerdbarts 8:676646c50aeb 429 }
sjoerdbarts 8:676646c50aeb 430 else
sjoerdbarts 8:676646c50aeb 431 {
sjoerdbarts 8:676646c50aeb 432 motor1_pwm.write(abs(MotorValue));
sjoerdbarts 8:676646c50aeb 433 }
sjoerdbarts 8:676646c50aeb 434 }
sjoerdbarts 8:676646c50aeb 435 else
sjoerdbarts 8:676646c50aeb 436 {
sjoerdbarts 8:676646c50aeb 437 if (MotorValue >=0)
sjoerdbarts 8:676646c50aeb 438 {
sjoerdbarts 8:676646c50aeb 439 motor2_dir=0;
sjoerdbarts 8:676646c50aeb 440 }
sjoerdbarts 8:676646c50aeb 441 else
sjoerdbarts 8:676646c50aeb 442 {
sjoerdbarts 8:676646c50aeb 443 motor2_dir=1;
sjoerdbarts 8:676646c50aeb 444 }
sjoerdbarts 8:676646c50aeb 445 if (fabs(MotorValue)>1){
sjoerdbarts 8:676646c50aeb 446 motor2_pwm.write(1);
sjoerdbarts 8:676646c50aeb 447 }
sjoerdbarts 8:676646c50aeb 448 else
sjoerdbarts 8:676646c50aeb 449 {
sjoerdbarts 8:676646c50aeb 450 motor2_pwm.write(abs(MotorValue));
sjoerdbarts 8:676646c50aeb 451 }
sjoerdbarts 8:676646c50aeb 452 }
sjoerdbarts 8:676646c50aeb 453 }
sjoerdbarts 8:676646c50aeb 454
sjoerdbarts 8:676646c50aeb 455 void BlinkLed(){
sjoerdbarts 8:676646c50aeb 456 led = not led;
sjoerdbarts 8:676646c50aeb 457 }
sjoerdbarts 8:676646c50aeb 458
sjoerdbarts 8:676646c50aeb 459 void Controller_motor()
sjoerdbarts 8:676646c50aeb 460 {
sjoerdbarts 8:676646c50aeb 461 // get the actual position
sjoerdbarts 8:676646c50aeb 462 double m1_Position = m1_GetPosition_cal();
sjoerdbarts 8:676646c50aeb 463 double m2_Position = m2_GetPosition_cal();
sjoerdbarts 8:676646c50aeb 464 // Set position scopes
sjoerdbarts 8:676646c50aeb 465 scope.set(0,x);
sjoerdbarts 8:676646c50aeb 466 scope.set(1,y);
sjoerdbarts 8:676646c50aeb 467 scope.set(2,Theta1);
sjoerdbarts 8:676646c50aeb 468 scope.set(3,Theta2);
sjoerdbarts 8:676646c50aeb 469 /*
sjoerdbarts 8:676646c50aeb 470 scope.set(0,Theta1);
sjoerdbarts 8:676646c50aeb 471 scope.set(1,m1_Position);
sjoerdbarts 8:676646c50aeb 472 scope.set(3,Theta2);
sjoerdbarts 8:676646c50aeb 473 scope.set(4,m2_Position);
sjoerdbarts 8:676646c50aeb 474 */
sjoerdbarts 8:676646c50aeb 475 //scope.set(2,m1_Position);
sjoerdbarts 8:676646c50aeb 476 //scope.set(4,m2_Position);
sjoerdbarts 8:676646c50aeb 477 // calc the error
sjoerdbarts 8:676646c50aeb 478 double m1_error = Theta1 - m1_Position;
sjoerdbarts 8:676646c50aeb 479 double m2_error = Theta2 - m2_Position;
sjoerdbarts 8:676646c50aeb 480 //scope.set(2,m1_error);
sjoerdbarts 8:676646c50aeb 481 //scope.set(6,m2_error);
sjoerdbarts 8:676646c50aeb 482 // calc motorvalues for controller;
sjoerdbarts 8:676646c50aeb 483 double m1_MotorValue = m1_PID(m1_error, Kp, Ki, Kd, Ts, N, m1_v1, m1_v2);
sjoerdbarts 8:676646c50aeb 484 double m2_MotorValue = m2_PID(m2_error, Kp, Ki, Kd, Ts, N, m2_v1, m2_v2);
sjoerdbarts 8:676646c50aeb 485 scope.set(4,m1_MotorValue);
sjoerdbarts 8:676646c50aeb 486 scope.set(5,m2_MotorValue);
sjoerdbarts 8:676646c50aeb 487 // Set the motorvalues
sjoerdbarts 8:676646c50aeb 488 SetMotor(1, m1_MotorValue);
sjoerdbarts 8:676646c50aeb 489 SetMotor(2, m2_MotorValue);
sjoerdbarts 8:676646c50aeb 490 // Set motorvalues for scope
sjoerdbarts 8:676646c50aeb 491 // Send data to HIDScope
sjoerdbarts 8:676646c50aeb 492 scope.send();
sjoerdbarts 8:676646c50aeb 493 }
sjoerdbarts 8:676646c50aeb 494
sjoerdbarts 8:676646c50aeb 495 void PotControl()
sjoerdbarts 8:676646c50aeb 496 {
sjoerdbarts 8:676646c50aeb 497 double Motor1_Value = (pot1.read() - 0.5f)/2.0f;
sjoerdbarts 8:676646c50aeb 498 double Motor2_Value = (pot2.read() - 0.5f)/2.0f;
sjoerdbarts 8:676646c50aeb 499 //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
sjoerdbarts 8:676646c50aeb 500 //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
sjoerdbarts 8:676646c50aeb 501 double m1_Position = m1_GetPosition();
sjoerdbarts 8:676646c50aeb 502 double m2_Position = m2_GetPosition();
sjoerdbarts 8:676646c50aeb 503 scope.set(0, Motor1_Value);
sjoerdbarts 8:676646c50aeb 504 scope.set(1, m1_Position);
sjoerdbarts 8:676646c50aeb 505 scope.set(2, Motor2_Value);
sjoerdbarts 8:676646c50aeb 506 scope.set(3, m2_Position);
sjoerdbarts 8:676646c50aeb 507 scope.send();
sjoerdbarts 8:676646c50aeb 508 // Write the motors
sjoerdbarts 8:676646c50aeb 509 SetMotor(1, Motor1_Value);
sjoerdbarts 8:676646c50aeb 510 SetMotor(2, Motor2_Value);
sjoerdbarts 8:676646c50aeb 511 }
sjoerdbarts 8:676646c50aeb 512
sjoerdbarts 8:676646c50aeb 513 void ResetEncoders()
sjoerdbarts 8:676646c50aeb 514 {
sjoerdbarts 8:676646c50aeb 515 m1_EncoderCW.reset();
sjoerdbarts 8:676646c50aeb 516 m1_EncoderCCW.reset();
sjoerdbarts 8:676646c50aeb 517 m2_EncoderCW.reset();
sjoerdbarts 8:676646c50aeb 518 m2_EncoderCCW.reset();
sjoerdbarts 8:676646c50aeb 519 }
sjoerdbarts 8:676646c50aeb 520
sjoerdbarts 8:676646c50aeb 521 void Button1Switch()
sjoerdbarts 8:676646c50aeb 522 {
sjoerdbarts 8:676646c50aeb 523 button1_value = not button1_value;
sjoerdbarts 8:676646c50aeb 524 }
sjoerdbarts 8:676646c50aeb 525
sjoerdbarts 8:676646c50aeb 526 void Button2Switch()
sjoerdbarts 8:676646c50aeb 527 {
sjoerdbarts 8:676646c50aeb 528 button2_value = not button2_value;
s1600907 1:e1267e72ade8 529 }
s1600907 0:c96cf9760185 530
s1600907 0:c96cf9760185 531 ////////////////////////// main loop ////////////////////////////////////////////
s1600907 0:c96cf9760185 532 int main()
sjoerdbarts 8:676646c50aeb 533 {
sjoerdbarts 8:676646c50aeb 534 // Setup
sjoerdbarts 8:676646c50aeb 535 // Set baud connection with PC
s1600907 0:c96cf9760185 536 pc.baud(SERIAL_BAUD);
sjoerdbarts 8:676646c50aeb 537 pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
s1600907 0:c96cf9760185 538
s1600907 0:c96cf9760185 539 /* making the biquad chain for filtering the emg
s1600907 0:c96cf9760185 540 notch and high pass */
s1600907 0:c96cf9760185 541 bqc.add( &bq1 ).add( &bq2 );
s1600907 0:c96cf9760185 542 bqc2.add( &bq4 ).add( &bq5 );
s1600907 0:c96cf9760185 543 bqc3.add( &bq7 ).add( &bq8 );
s1600907 0:c96cf9760185 544
sjoerdbarts 8:676646c50aeb 545 // Setup Tickers
sjoerdbarts 8:676646c50aeb 546 Ticker SampleTicker;
sjoerdbarts 8:676646c50aeb 547 Ticker CalculationsTicker;
sjoerdbarts 8:676646c50aeb 548 Ticker BlinkLedTicker;
sjoerdbarts 8:676646c50aeb 549 Ticker PIDControlTicker;
sjoerdbarts 8:676646c50aeb 550 Ticker PotControlTicker;
sjoerdbarts 7:bafc32b576c4 551
sjoerdbarts 8:676646c50aeb 552 // Setup Blinking LED to blink every 0.5 sec
sjoerdbarts 8:676646c50aeb 553 led = 1;
sjoerdbarts 8:676646c50aeb 554 BlinkLedTicker.attach(BlinkLed,0.5);
sjoerdbarts 8:676646c50aeb 555
sjoerdbarts 8:676646c50aeb 556 // Setup motor PWM speeds
sjoerdbarts 8:676646c50aeb 557 motor1_pwm.period(1.0/1000);
sjoerdbarts 8:676646c50aeb 558 motor2_pwm.period(1.0/1000);
sjoerdbarts 7:bafc32b576c4 559
sjoerdbarts 8:676646c50aeb 560 // Setup Interruptin.fall
sjoerdbarts 8:676646c50aeb 561 button1.fall(Button1Switch);
sjoerdbarts 8:676646c50aeb 562 //button2.fall(xxx);
sjoerdbarts 8:676646c50aeb 563 //button3.fall(xxx);
sjoerdbarts 8:676646c50aeb 564 //button4.fall(xxx);
s1600907 0:c96cf9760185 565
sjoerdbarts 8:676646c50aeb 566 // Actual code starts here
sjoerdbarts 8:676646c50aeb 567 // Start positioning the arms
sjoerdbarts 8:676646c50aeb 568 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 8:676646c50aeb 569 pc.printf("\r\n Press Button 1 to start positioning the arms using PotControl\r\n");
sjoerdbarts 8:676646c50aeb 570 pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
sjoerdbarts 8:676646c50aeb 571 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 8:676646c50aeb 572 while (button1_value == false){}
sjoerdbarts 8:676646c50aeb 573 PotControlTicker.attach(&PotControl,CONTROLLER_TS);
s1600907 0:c96cf9760185 574
sjoerdbarts 8:676646c50aeb 575 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 8:676646c50aeb 576 pc.printf("\r\n When done positioning, press Button 1 to detach Potcontrol");
sjoerdbarts 8:676646c50aeb 577 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 8:676646c50aeb 578 while (button1_value == true){}
sjoerdbarts 8:676646c50aeb 579
sjoerdbarts 8:676646c50aeb 580 // Detach potcontrol
sjoerdbarts 8:676646c50aeb 581 PotControlTicker.detach();
sjoerdbarts 8:676646c50aeb 582
sjoerdbarts 8:676646c50aeb 583 // Set motors to 0
sjoerdbarts 8:676646c50aeb 584 SetMotor(1,0.0);
sjoerdbarts 8:676646c50aeb 585 SetMotor(2,0.0);
sjoerdbarts 8:676646c50aeb 586 // Wait a bit to let the movement stop
sjoerdbarts 8:676646c50aeb 587 wait(0.5);
sjoerdbarts 8:676646c50aeb 588 // Reset the encoders to set the 0 position
sjoerdbarts 8:676646c50aeb 589 ResetEncoders();
sjoerdbarts 8:676646c50aeb 590
sjoerdbarts 8:676646c50aeb 591 // PID control starts
sjoerdbarts 8:676646c50aeb 592 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 8:676646c50aeb 593 pc.printf("\r\n When done positioning, press button SW3 to start potmeter PID control");
sjoerdbarts 8:676646c50aeb 594 pc.printf("\r\n Make sure both potentiometers are positioned halfway!!! \r\n");
sjoerdbarts 8:676646c50aeb 595 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 8:676646c50aeb 596 while (button1_value == false){}
sjoerdbarts 8:676646c50aeb 597
sjoerdbarts 8:676646c50aeb 598 // Attach sample, calc and control tickers
sjoerdbarts 8:676646c50aeb 599 SampleTicker.attach(&sample, SAMPLE_TS);
sjoerdbarts 8:676646c50aeb 600 CalculationsTicker.attach(&CalculationsForTheta,CALC_TS);
sjoerdbarts 8:676646c50aeb 601 PIDControlTicker.attach(&Controller_motor, CONTROLLER_TS); // 100 Hz
sjoerdbarts 8:676646c50aeb 602 while(true){}
s1600907 0:c96cf9760185 603 }