Biorobotics_group_2 / Mbed 2 deprecated CompleteFunction

Alle subfiles samengevoegd

Dependencies:   HIDScope biquadFilter mbed MODSERIAL FastPWM QEI

main.cpp@7:bafc32b576c4, 2016-10-31 (annotated)

Committer:
sjoerdbarts
Date:
Mon Oct 31 10:42:56 2016 +0000
Revision:
7:bafc32b576c4
Parent:
6:00011220b82b
Child:
8:676646c50aeb
Translated some comments

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 6:00011220b82b 29 // Set motor Pinouts
sjoerdbarts 6:00011220b82b 30 DigitalOut motor1_dir(D4);
sjoerdbarts 6:00011220b82b 31 FastPWM motor1_pwm(D5);
sjoerdbarts 6:00011220b82b 32 DigitalOut motor2_dir(D7);
sjoerdbarts 6:00011220b82b 33 FastPWM motor2_pwm(D6);
sjoerdbarts 6:00011220b82b 34
sjoerdbarts 6:00011220b82b 35 // Set LED pins
sjoerdbarts 6:00011220b82b 36 DigitalOut led(LED_RED);
sjoerdbarts 6:00011220b82b 37
sjoerdbarts 6:00011220b82b 38 // Set HID scope
sjoerdbarts 6:00011220b82b 39 HIDScope scope(6);
sjoerdbarts 6:00011220b82b 40
sjoerdbarts 6:00011220b82b 41 // Set encoder
sjoerdbarts 6:00011220b82b 42 QEI m1_EncoderCW(D10,D11,NC,32);
sjoerdbarts 6:00011220b82b 43 QEI m1_EncoderCCW(D11,D10,NC,32);
sjoerdbarts 6:00011220b82b 44 QEI m2_EncoderCW(D13,D12,NC,32);
sjoerdbarts 6:00011220b82b 45 QEI m2_EncoderCCW(D12,D13,NC,32);
s1600907 0:c96cf9760185 46
sjoerdbarts 7:bafc32b576c4 47 // Constants for control
sjoerdbarts 7:bafc32b576c4 48 volatile const int COUNTS_PER_REV = 8400;
sjoerdbarts 7:bafc32b576c4 49 volatile const float DEGREES_PER_PULSE = 8400 / 360;
sjoerdbarts 7:bafc32b576c4 50 volatile const float CONTROLLER_TS = 0.01; // 1000 Hz
sjoerdbarts 7:bafc32b576c4 51
sjoerdbarts 7:bafc32b576c4 52 // Set initial Kp and Ki
sjoerdbarts 7:bafc32b576c4 53 volatile double Kp = 0.01; // last known good Kp: 0.01
sjoerdbarts 7:bafc32b576c4 54 volatile double Ki = 0.0; // last known good Ki: 0.0025
sjoerdbarts 7:bafc32b576c4 55 volatile double Kd = 0.0; // last known good Kd: 0.0
sjoerdbarts 7:bafc32b576c4 56
sjoerdbarts 7:bafc32b576c4 57 volatile double Ts = 0.01;
sjoerdbarts 7:bafc32b576c4 58 volatile double N = 100;
sjoerdbarts 7:bafc32b576c4 59
sjoerdbarts 7:bafc32b576c4 60 // Memory values for controllers
sjoerdbarts 7:bafc32b576c4 61 double m1_v1 = 0, m1_v2 = 0;
sjoerdbarts 7:bafc32b576c4 62 double m2_v1 = 0, m2_v2 = 0;
s1600907 0:c96cf9760185 63
s1600907 0:c96cf9760185 64 //BiQuad
s1600907 0:c96cf9760185 65 // making the biquads and chains; imported from matlab
s1600907 0:c96cf9760185 66 BiQuadChain bqc; //Chain for right biceps
s1600907 0:c96cf9760185 67 BiQuadChain bqc2; //Chain for left biceps
s1600907 0:c96cf9760185 68 BiQuadChain bqc3; //Chain for Upper leg
s1600907 0:c96cf9760185 69 // 1 to 3 are for right biceps
s1600907 0:c96cf9760185 70 BiQuad bq1( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 71 BiQuad bq2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 72 BiQuad bq3( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
s1600907 0:c96cf9760185 73 // 4 to 6 are for left biceps
s1600907 0:c96cf9760185 74 BiQuad bq4( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 75 BiQuad bq5( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 76 BiQuad bq6( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
s1600907 0:c96cf9760185 77 // 7 to 9 are for upper leg
s1600907 0:c96cf9760185 78 BiQuad bq7( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 79 BiQuad bq8( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 80 BiQuad bq9( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
s1600907 0:c96cf9760185 81
sjoerdbarts 7:bafc32b576c4 82 // Variables for filter
sjoerdbarts 7:bafc32b576c4 83 // muscle EMG < treshhold
s1600907 0:c96cf9760185 84 volatile bool BicepsLeft = false;
s1600907 0:c96cf9760185 85 volatile bool BicepsRight = false;
s1600907 0:c96cf9760185 86 volatile bool Leg = false;
s1600907 0:c96cf9760185 87
sjoerdbarts 7:bafc32b576c4 88 // Variables for MotorHoekBerekenen (MotorAngleCalculation)
s1600907 1:e1267e72ade8 89 volatile double x = 0.0; // beginpositie x en y
s1600907 1:e1267e72ade8 90 volatile double y = 305.5;
s1600907 0:c96cf9760185 91 volatile const double pi = 3.14159265359;
s1600907 3:c524afedf863 92 volatile double Theta1Gear = 60.0; // Beginpositie op 60 graden
s1600907 3:c524afedf863 93 volatile double Theta2Gear = 60.0;
s1600907 1:e1267e72ade8 94 volatile double Theta1 = Theta1Gear*42/12; // Beginpositie van MotorHoek
s1600907 1:e1267e72ade8 95 volatile double Theta2 = Theta2Gear*42/12;
s1600907 0:c96cf9760185 96 double Fr_Speed_Theta = 100.0; // frequentie in Hz waarmee theta wordt uigerekend
s1600907 0:c96cf9760185 97 bool Calibration = true; // beginnen met calibreren
s1600907 0:c96cf9760185 98 volatile bool Stepper_State = false; // false = we zijn niet bezig met flippen
s1600907 0:c96cf9760185 99
sjoerdbarts 7:bafc32b576c4 100 /////////////////////// functions for filters /////////////////////////////////////////////
s1600907 0:c96cf9760185 101 void sample()
s1600907 0:c96cf9760185 102 {
s1600907 0:c96cf9760185 103 /* Sample function
s1600907 0:c96cf9760185 104 samples the emg0 (of the rightBiceps)
s1600907 0:c96cf9760185 105 samples the emg1 (of the leftBiceps)
s1600907 0:c96cf9760185 106 samples the emg2 (of the Upper leg)
s1600907 0:c96cf9760185 107 filter the singals
s1600907 0:c96cf9760185 108 sends it to HIDScope
s1600907 0:c96cf9760185 109 */
s1600907 0:c96cf9760185 110
s1600907 0:c96cf9760185 111 // Rechts Biceps
s1600907 0:c96cf9760185 112 double inRechts = emg0.read(); // EMG signal
s1600907 0:c96cf9760185 113 double FilterRechts = bqc.step(inRechts); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 114 double RectifyRechts = fabs(FilterRechts); // Rectify
s1600907 0:c96cf9760185 115 double outRechts = bq3.step(RectifyRechts); // Low pass
s1600907 2:92593c9d6146 116
s1600907 0:c96cf9760185 117 // Links Biceps
s1600907 0:c96cf9760185 118 double inLinks = emg1.read(); // EMG signal
s1600907 0:c96cf9760185 119 double FilterLinks = bqc2.step(inLinks); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 120 double RectifyLinks = fabs(FilterLinks); // Rectify
s1600907 0:c96cf9760185 121 double outLinks = bq6.step(RectifyLinks); // Low pass
s1600907 0:c96cf9760185 122
s1600907 0:c96cf9760185 123 // Upper leg
s1600907 0:c96cf9760185 124 double inBeen = emg2.read(); // EMG signal
s1600907 0:c96cf9760185 125 double FilterBeen = bqc3.step(inBeen); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 126 double RectifyBeen = fabs(FilterBeen); // Rectify
s1600907 2:92593c9d6146 127 double outBeen = bq9.step(RectifyBeen); // Low pass
s1600907 0:c96cf9760185 128
s1600907 0:c96cf9760185 129
s1600907 0:c96cf9760185 130 /* EMG signal in channel 0 (the first channel)
s1600907 0:c96cf9760185 131 and the filtered EMG signal in channel 1 (the second channel)
s1600907 0:c96cf9760185 132 of the HIDscope */
s1600907 5:b4a0301ec8cc 133 /*scope.set(0,inRechts);
s1600907 0:c96cf9760185 134 scope.set(1,outRechts);
s1600907 2:92593c9d6146 135 scope.set(2,inLinks);
s1600907 0:c96cf9760185 136 scope.set(3,outLinks);
s1600907 0:c96cf9760185 137 scope.set(4,inBeen);
s1600907 0:c96cf9760185 138 scope.set(5,outBeen);
s1600907 5:b4a0301ec8cc 139 */
s1600907 0:c96cf9760185 140 // To indicate that the function is working, the LED is on
s1600907 1:e1267e72ade8 141 if (outRechts >= 0.04){
s1600907 0:c96cf9760185 142 BicepsRight = true;
s1600907 0:c96cf9760185 143 }
s1600907 0:c96cf9760185 144 else{
s1600907 0:c96cf9760185 145 BicepsRight = false;
s1600907 0:c96cf9760185 146 }
s1600907 2:92593c9d6146 147
s1600907 0:c96cf9760185 148 // If Biceps links is actuated then:
s1600907 1:e1267e72ade8 149 if (outLinks >= 0.04){
s1600907 0:c96cf9760185 150 BicepsLeft = true;
s1600907 0:c96cf9760185 151 }
s1600907 0:c96cf9760185 152 else{
s1600907 0:c96cf9760185 153 BicepsLeft = false;
s1600907 0:c96cf9760185 154 }
s1600907 0:c96cf9760185 155 // If upper leg is actuated then:
s1600907 0:c96cf9760185 156 if (outBeen >= 0.01){
s1600907 0:c96cf9760185 157 Leg = true;
s1600907 0:c96cf9760185 158 }
s1600907 0:c96cf9760185 159 else{
s1600907 0:c96cf9760185 160 Leg = false;
s1600907 0:c96cf9760185 161 }
s1600907 0:c96cf9760185 162 }
s1600907 0:c96cf9760185 163
sjoerdbarts 7:bafc32b576c4 164 ///////////////////// functions for "motorhoekberekenen" //////////////////////////
sjoerdbarts 7:bafc32b576c4 165 // save previous x
s1600907 0:c96cf9760185 166 double Calc_Prev_x () {
s1600907 0:c96cf9760185 167 double Prev_x = x;
s1600907 0:c96cf9760185 168 //pc.printf("prev x = %f\r\n", Prev_x);
s1600907 0:c96cf9760185 169 return Prev_x;
s1600907 0:c96cf9760185 170 }
s1600907 0:c96cf9760185 171
sjoerdbarts 7:bafc32b576c4 172 // save previous y
s1600907 0:c96cf9760185 173 double Calc_Prev_y () {
s1600907 0:c96cf9760185 174 double Prev_y = y;
s1600907 0:c96cf9760185 175 //pc.printf("prev y = %f\r\n", Prev_y);
s1600907 0:c96cf9760185 176 return Prev_y;
s1600907 0:c96cf9760185 177 }
s1600907 0:c96cf9760185 178
sjoerdbarts 7:bafc32b576c4 179 // save previous Theta1Gear
s1600907 4:05c5da1c8b08 180 double Calc_Prev_Theta1_Gear () {
s1600907 4:05c5da1c8b08 181 double Prev_Theta1_Gear = Theta1Gear;
s1600907 4:05c5da1c8b08 182 return Prev_Theta1_Gear;
s1600907 4:05c5da1c8b08 183 }
s1600907 4:05c5da1c8b08 184
sjoerdbarts 7:bafc32b576c4 185 // save previous Theta2Gear
s1600907 4:05c5da1c8b08 186 double Calc_Prev_Theta2_Gear () {
s1600907 4:05c5da1c8b08 187 double Prev_Theta2_Gear = Theta2Gear;
s1600907 4:05c5da1c8b08 188 return Prev_Theta2_Gear;
s1600907 4:05c5da1c8b08 189 }
s1600907 4:05c5da1c8b08 190
s1600907 2:92593c9d6146 191 void FunctieFlipper(); // declarate function, function discribed below
s1600907 0:c96cf9760185 192 // berekenen van de nieuwe x en y waardes
sjoerdbarts 7:bafc32b576c4 193 bool CalcXY (bool BicepsLeft, bool BicepsRight, bool Leg, bool Stepper_State)
sjoerdbarts 7:bafc32b576c4 194 {
s1600907 1:e1267e72ade8 195 double Step = 5/Fr_Speed_Theta ; //10 mm per seconde afleggen
s1600907 0:c96cf9760185 196
s1600907 0:c96cf9760185 197 if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) {
s1600907 0:c96cf9760185 198 Stepper_State = true; // we zijn aan het flipper dus geen andere dingen doen
s1600907 2:92593c9d6146 199 FunctieFlipper() ;
s1600907 0:c96cf9760185 200 }
s1600907 0:c96cf9760185 201 else if (BicepsLeft==true && BicepsRight==false && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 202 x = x - Step;
s1600907 0:c96cf9760185 203 }
s1600907 0:c96cf9760185 204 else if (BicepsLeft==false && BicepsRight==true && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 205 x = x + Step; // naar Right bewegen
s1600907 0:c96cf9760185 206 }
s1600907 0:c96cf9760185 207 else if (BicepsLeft==true && BicepsRight==true && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 208 y = y + Step; // naar voren bewegen
s1600907 0:c96cf9760185 209 }
s1600907 0:c96cf9760185 210 else if (BicepsLeft==false && BicepsRight==true && Leg==true && Stepper_State==false) {
s1600907 0:c96cf9760185 211 y = y - Step; // naar achter bewegen
s1600907 0:c96cf9760185 212 }
s1600907 0:c96cf9760185 213 else if (BicepsLeft==false && BicepsRight==false && Leg==false || Stepper_State==true) {
s1600907 0:c96cf9760185 214 }
s1600907 0:c96cf9760185 215
s1600907 0:c96cf9760185 216 // Grenswaardes LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
s1600907 0:c96cf9760185 217 if (x > 200) {
s1600907 0:c96cf9760185 218 x = 200;
s1600907 0:c96cf9760185 219 }
s1600907 0:c96cf9760185 220 else if (x < -200) {
s1600907 0:c96cf9760185 221 x = -200;
s1600907 0:c96cf9760185 222 }
s1600907 0:c96cf9760185 223 if (y > 306) {
s1600907 0:c96cf9760185 224 y = 306;
s1600907 0:c96cf9760185 225 }
s1600907 0:c96cf9760185 226 else if (y < 50) {
s1600907 0:c96cf9760185 227 y = 50; // GOKJE, UITPROBEREN
s1600907 0:c96cf9760185 228 }
s1600907 5:b4a0301ec8cc 229 //pc.printf("x = %f, y = %f\r\n", x, y);
s1600907 1:e1267e72ade8 230
s1600907 2:92593c9d6146 231 //scope.set(2,x);
s1600907 2:92593c9d6146 232 //scope.set(3,y);
s1600907 0:c96cf9760185 233
s1600907 0:c96cf9760185 234 return Stepper_State;
s1600907 0:c96cf9760185 235 }
s1600907 0:c96cf9760185 236
s1600907 0:c96cf9760185 237 // diagonaal berekenen voor linker arm
s1600907 0:c96cf9760185 238 double CalcDia1 (double x, double y) {
s1600907 0:c96cf9760185 239 double a = 50.0; // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden) KEERTJE NAMETEN
s1600907 0:c96cf9760185 240 double BV1 = sqrt(pow((a+x),2) + pow(y,2)); // diagonaal (afstand van armas tot locatie) berekenen
s1600907 0:c96cf9760185 241 double Dia1 = pow(BV1,2)/(2*BV1); // berekenen van de afstand oorsprong tot diagonaal
s1600907 0:c96cf9760185 242
s1600907 0:c96cf9760185 243 //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia1, x, y);
s1600907 0:c96cf9760185 244 return Dia1;
s1600907 0:c96cf9760185 245 }
s1600907 0:c96cf9760185 246
s1600907 0:c96cf9760185 247 // diagonaal berekenen voor rechter arm
s1600907 0:c96cf9760185 248 double CalcDia2 (double x, double y) {
s1600907 0:c96cf9760185 249 double a = 50.0;
s1600907 0:c96cf9760185 250 double BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm
s1600907 0:c96cf9760185 251 double Dia2 = pow(BV2,2)/(2*BV2);
s1600907 0:c96cf9760185 252
s1600907 0:c96cf9760185 253 //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia2, x, y);
s1600907 0:c96cf9760185 254 return Dia2;
s1600907 0:c96cf9760185 255 }
s1600907 0:c96cf9760185 256
s1600907 0:c96cf9760185 257 // calculate Theta1
s1600907 4:05c5da1c8b08 258 void CalcTheta1 (double Dia1) {
s1600907 0:c96cf9760185 259 double a = 50.0;
s1600907 0:c96cf9760185 260 double Bar = 200.0; // lengte van de armen
s1600907 0:c96cf9760185 261
s1600907 0:c96cf9760185 262 // Hoek berekenen van het grote tandwiel (gear)
s1600907 0:c96cf9760185 263 if (x > -a) {
s1600907 0:c96cf9760185 264 Theta1Gear = pi - atan(y/(x+a)) - acos(Dia1/Bar);
s1600907 0:c96cf9760185 265 }
s1600907 0:c96cf9760185 266 else if (x > -a) {
s1600907 0:c96cf9760185 267 Theta1Gear = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
s1600907 0:c96cf9760185 268 }
s1600907 0:c96cf9760185 269 else { // als x=-a
s1600907 0:c96cf9760185 270 Theta1Gear = 0.5*pi - acos(Dia1/Bar);
s1600907 0:c96cf9760185 271 }
s1600907 3:c524afedf863 272 Theta1Gear = Theta1Gear*180.0/pi; // veranderen van radialen naar graden
s1600907 0:c96cf9760185 273
s1600907 0:c96cf9760185 274 // omrekenen van grote tandwiel naar motortandwiel
s1600907 0:c96cf9760185 275 Theta1 = Theta1Gear*42.0/12.0; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
s1600907 0:c96cf9760185 276
s1600907 5:b4a0301ec8cc 277 pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta1, Theta1Gear);
s1600907 0:c96cf9760185 278 }
s1600907 0:c96cf9760185 279
s1600907 4:05c5da1c8b08 280 void CalcTheta2 (double Dia2) {
s1600907 0:c96cf9760185 281 double a = 50.0;
s1600907 0:c96cf9760185 282 double Bar = 200.0; // lengte van de armen
s1600907 0:c96cf9760185 283 double Prev_Theta2_Gear = Theta2Gear;
s1600907 4:05c5da1c8b08 284
s1600907 0:c96cf9760185 285 // Hoek berekenen van het grote tandwiel (gear)
s1600907 0:c96cf9760185 286 if (x < a) {
s1600907 0:c96cf9760185 287 Theta2Gear = pi + atan(y/(x-a)) - acos(Dia2/Bar);
s1600907 0:c96cf9760185 288 }
s1600907 0:c96cf9760185 289 else if (x > a) {
s1600907 0:c96cf9760185 290 Theta2Gear = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
s1600907 0:c96cf9760185 291 }
s1600907 0:c96cf9760185 292 else { // als x=a
s1600907 0:c96cf9760185 293 Theta2Gear = 0.5*pi - acos(Dia2/Bar);
s1600907 0:c96cf9760185 294 }
s1600907 3:c524afedf863 295 Theta2Gear = Theta2Gear*180/pi; // veranderen van radialen naar graden
s1600907 0:c96cf9760185 296
s1600907 4:05c5da1c8b08 297 // omrekenen van grote tandwiel naar motortandwiel
s1600907 4:05c5da1c8b08 298 Theta2 = Theta2Gear*42.0/12.0; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
s1600907 4:05c5da1c8b08 299
s1600907 5:b4a0301ec8cc 300 pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
s1600907 4:05c5da1c8b08 301 }
s1600907 4:05c5da1c8b08 302
s1600907 4:05c5da1c8b08 303 // als een van de hoeken te groot wordt, zet dan alles terug naar de vorige positie
s1600907 4:05c5da1c8b08 304 void AngleLimits (double Prev_Theta1_Gear, double Prev_Theta2_Gear, double Prev_x, double Prev_y) {
s1600907 4:05c5da1c8b08 305 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 306
s1600907 4:05c5da1c8b08 307 if (Theta1Gear >= MaxThetaGear || Theta2Gear >= MaxThetaGear) {
s1600907 4:05c5da1c8b08 308 Theta1Gear = Prev_Theta1_Gear;
s1600907 0:c96cf9760185 309 Theta2Gear = Prev_Theta2_Gear;
s1600907 0:c96cf9760185 310 x = Prev_x;
s1600907 0:c96cf9760185 311 y = Prev_y;
s1600907 4:05c5da1c8b08 312
s1600907 4:05c5da1c8b08 313 Theta1 = Theta1Gear*42.0/12.0;
s1600907 4:05c5da1c8b08 314 Theta2 = Theta2Gear*42.0/12.0;
s1600907 0:c96cf9760185 315 }
s1600907 5:b4a0301ec8cc 316 pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
s1600907 0:c96cf9760185 317 }
s1600907 0:c96cf9760185 318
s1600907 0:c96cf9760185 319 void CalculationsForTheta () {
s1600907 1:e1267e72ade8 320 sample();
s1600907 0:c96cf9760185 321 double Prev_x = Calc_Prev_x ();
s1600907 0:c96cf9760185 322 double Prev_y = Calc_Prev_y ();
s1600907 4:05c5da1c8b08 323 double Calc_Prev_Theta1_Gear ();
s1600907 4:05c5da1c8b08 324 double Calc_Prev_Theta2_Gear ();
s1600907 0:c96cf9760185 325 bool Stepper_State = CalcXY (BicepsLeft, BicepsRight, Leg, Stepper_State);
s1600907 0:c96cf9760185 326 double Dia1 = CalcDia1 (x, y);
s1600907 0:c96cf9760185 327 double Dia2 = CalcDia2 (x, y);
s1600907 4:05c5da1c8b08 328 CalcTheta1 (Dia1);
s1600907 4:05c5da1c8b08 329 CalcTheta2 (Dia2);
s1600907 4:05c5da1c8b08 330 AngleLimits (Theta1Gear, Theta2Gear, Prev_x, Prev_y);
s1600907 2:92593c9d6146 331
s1600907 5:b4a0301ec8cc 332 //scope.send();
s1600907 0:c96cf9760185 333 }
s1600907 0:c96cf9760185 334
s1600907 0:c96cf9760185 335 // als de button ingedrukt wordt dan stoppen we met calibreren
s1600907 0:c96cf9760185 336 void EndCalibration () {
s1600907 0:c96cf9760185 337 Calibration = false;
s1600907 0:c96cf9760185 338 }
s1600907 0:c96cf9760185 339
s1600907 2:92593c9d6146 340 //////////////// functies voor aansturing motoren ///////////////////////////////
s1600907 2:92593c9d6146 341 void FunctieFlipper () {
s1600907 2:92593c9d6146 342 Stepper_State = false;
s1600907 1:e1267e72ade8 343 }
s1600907 0:c96cf9760185 344
s1600907 0:c96cf9760185 345 ////////////////////////// main loop ////////////////////////////////////////////
s1600907 0:c96cf9760185 346 int main()
s1600907 0:c96cf9760185 347 {
s1600907 0:c96cf9760185 348 pc.baud(SERIAL_BAUD);
s1600907 0:c96cf9760185 349 pc.printf("\r\n Nieuwe code uitproberen :) \r\n");
s1600907 0:c96cf9760185 350
s1600907 0:c96cf9760185 351 /* making the biquad chain for filtering the emg
s1600907 0:c96cf9760185 352 notch and high pass */
s1600907 0:c96cf9760185 353 bqc.add( &bq1 ).add( &bq2 );
s1600907 0:c96cf9760185 354 bqc2.add( &bq4 ).add( &bq5 );
s1600907 0:c96cf9760185 355 bqc3.add( &bq7 ).add( &bq8 );
s1600907 0:c96cf9760185 356
sjoerdbarts 7:bafc32b576c4 357 // Tickers
sjoerdbarts 7:bafc32b576c4 358 Ticker sample_timer;
sjoerdbarts 7:bafc32b576c4 359 Ticker TickerCalculationsForTheta;
sjoerdbarts 7:bafc32b576c4 360
sjoerdbarts 7:bafc32b576c4 361
s1600907 0:c96cf9760185 362 // Calibreren
s1600907 0:c96cf9760185 363 while (Calibration == true) {
s1600907 1:e1267e72ade8 364 //potmeter dingen aanpassen
s1600907 0:c96cf9760185 365
s1600907 0:c96cf9760185 366 pc.printf("calibreren is bezig\r\n");
sjoerdbarts 6:00011220b82b 367 button1.fall(&EndCalibration);
s1600907 0:c96cf9760185 368 }
s1600907 0:c96cf9760185 369
s1600907 0:c96cf9760185 370 /**Attach the 'sample' function to the timer 'sample_timer'.
s1600907 0:c96cf9760185 371 * this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
s1600907 0:c96cf9760185 372 */
s1600907 1:e1267e72ade8 373 // sample_timer.attach(&sample, 0.002);
s1600907 2:92593c9d6146 374
s1600907 0:c96cf9760185 375 // ticker voor hoek berekenen aanzetten
s1600907 5:b4a0301ec8cc 376 TickerCalculationsForTheta.attach(&CalculationsForTheta,1); //0.002
s1600907 2:92593c9d6146 377
s1600907 0:c96cf9760185 378 //empty loop, sample() is executed periodically
s1600907 1:e1267e72ade8 379 while(1) {
s1600907 1:e1267e72ade8 380
s1600907 1:e1267e72ade8 381 }
s1600907 0:c96cf9760185 382 }