Aukie Hooglugt / Mbed 2 deprecated PROJECT_FINAL_VERSLAG

verslag

Dependencies:   Encoder HIDScope MODSERIAL- mbed-dsp mbed

Fork of PROJECT_FINAL by Aukie Hooglugt

PROJECT_main.cpp@7:ca1ade91bd14, 2014-11-03 (annotated)

Committer:
Hooglugt
Date:
Mon Nov 03 18:22:20 2014 +0000
Revision:
7:ca1ade91bd14
Parent:
6:14051758db6f
Child:
8:75980dc35763
inb4 arvid

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hooglugt 0:99cbc87af37c 1 #include "mbed.h"
Hooglugt 0:99cbc87af37c 2 #include "MODSERIAL.h"
Hooglugt 0:99cbc87af37c 3 #include "HIDScope.h"
Hooglugt 0:99cbc87af37c 4 #include "arm_math.h"
Hooglugt 0:99cbc87af37c 5 #include "encoder.h"
Hooglugt 0:99cbc87af37c 6
Hooglugt 0:99cbc87af37c 7 #define TSAMP 0.001 // sample freq encoder motor
Hooglugt 0:99cbc87af37c 8 #define TIMEB4NEXTCHOICE 1 // sec keuzelampje blijft aan
Hooglugt 1:d44a866de64f 9 #define TIMEBETWEENBLINK 100 // sec voor volgende blink
Hooglugt 0:99cbc87af37c 10 #define TSAMP_EMG 0.002 //sample frequency emg
Hooglugt 7:ca1ade91bd14 11 #define KALIBRATIONTIME 500 // 10 sec voor bepalen van maximale biceps/triceps waarde
Hooglugt 1:d44a866de64f 12 #define FACTOR 0.6 //factor*max_waarde = threshold emg
Hooglugt 7:ca1ade91bd14 13
Hooglugt 0:99cbc87af37c 14 //Define objects
Hooglugt 7:ca1ade91bd14 15
Hooglugt 7:ca1ade91bd14 16 HIDScope scope(2);
Hooglugt 7:ca1ade91bd14 17
Hooglugt 0:99cbc87af37c 18 AnalogIn emg0(PTB1); //Analog input biceps
Hooglugt 0:99cbc87af37c 19 AnalogIn emg1(PTB2); //Analog input triceps
Hooglugt 0:99cbc87af37c 20
Hooglugt 0:99cbc87af37c 21 Ticker log_timer; //sample emg
Hooglugt 1:d44a866de64f 22 Ticker blink; //ledjes aan/uit
Hooglugt 1:d44a866de64f 23 Ticker blink2; //extra tikker zodat kalbi en kaltri tegelijkertijd aankunnen
Hooglugt 0:99cbc87af37c 24 Ticker looptimer; //motor regelaar
Hooglugt 0:99cbc87af37c 25
Hooglugt 0:99cbc87af37c 26 MODSERIAL pc(USBTX,USBRX);
Hooglugt 0:99cbc87af37c 27
Hooglugt 0:99cbc87af37c 28 arm_biquad_casd_df1_inst_f32 bihighpass;
Hooglugt 0:99cbc87af37c 29 float bihighpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
Hooglugt 0:99cbc87af37c 30 float bihighpass_states[4];
Hooglugt 0:99cbc87af37c 31
Hooglugt 0:99cbc87af37c 32 arm_biquad_casd_df1_inst_f32 binotch;
Hooglugt 0:99cbc87af37c 33 float binotch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
Hooglugt 0:99cbc87af37c 34 float binotch_states[4];
Hooglugt 0:99cbc87af37c 35
Hooglugt 0:99cbc87af37c 36 arm_biquad_casd_df1_inst_f32 trihighpass;
Hooglugt 0:99cbc87af37c 37 float trihighpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
Hooglugt 0:99cbc87af37c 38 float trihighpass_states[4];
Hooglugt 0:99cbc87af37c 39
Hooglugt 0:99cbc87af37c 40 arm_biquad_casd_df1_inst_f32 trinotch;
Hooglugt 0:99cbc87af37c 41 float trinotch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
Hooglugt 0:99cbc87af37c 42 float trinotch_states[4];
Hooglugt 0:99cbc87af37c 43
Hooglugt 0:99cbc87af37c 44 float bi_result = 0;
Hooglugt 0:99cbc87af37c 45 float tri_result = 0;
Hooglugt 0:99cbc87af37c 46
Hooglugt 0:99cbc87af37c 47 float bi_max = 0;
Hooglugt 0:99cbc87af37c 48 float tri_max = 0;
Hooglugt 0:99cbc87af37c 49
Hooglugt 0:99cbc87af37c 50 // variables for biceps MAF
Hooglugt 0:99cbc87af37c 51 float y0 = 0;
Hooglugt 0:99cbc87af37c 52 float y1 = 0;
Hooglugt 0:99cbc87af37c 53 float y2 = 0;
Hooglugt 0:99cbc87af37c 54 float y3 = 0;
Hooglugt 0:99cbc87af37c 55 float y4 = 0;
Hooglugt 0:99cbc87af37c 56 float y5 = 0;
Hooglugt 0:99cbc87af37c 57 float y6 = 0;
Hooglugt 0:99cbc87af37c 58 float y7 = 0;
Hooglugt 0:99cbc87af37c 59 float y8 = 0;
Hooglugt 0:99cbc87af37c 60 float y9 = 0;
Hooglugt 0:99cbc87af37c 61
Hooglugt 0:99cbc87af37c 62 // variables for triceps MAF
Hooglugt 0:99cbc87af37c 63 float x0 = 0;
Hooglugt 0:99cbc87af37c 64 float x1 = 0;
Hooglugt 0:99cbc87af37c 65 float x2 = 0;
Hooglugt 0:99cbc87af37c 66 float x3 = 0;
Hooglugt 0:99cbc87af37c 67 float x4 = 0;
Hooglugt 0:99cbc87af37c 68 float x5 = 0;
Hooglugt 0:99cbc87af37c 69 float x6 = 0;
Hooglugt 0:99cbc87af37c 70 float x7 = 0;
Hooglugt 0:99cbc87af37c 71 float x8 = 0;
Hooglugt 0:99cbc87af37c 72 float x9 = 0;
Hooglugt 0:99cbc87af37c 73
Hooglugt 0:99cbc87af37c 74 //LED interface
Hooglugt 0:99cbc87af37c 75 DigitalOut dir1(PTA1);
Hooglugt 0:99cbc87af37c 76 DigitalOut dir2(PTA2);
Hooglugt 0:99cbc87af37c 77 DigitalOut dir3(PTD4);
Hooglugt 0:99cbc87af37c 78 DigitalOut for1(PTA12);
Hooglugt 0:99cbc87af37c 79 DigitalOut for2(PTA13);
Hooglugt 0:99cbc87af37c 80 DigitalOut for3(PTD1);
Hooglugt 0:99cbc87af37c 81
Hooglugt 0:99cbc87af37c 82 uint8_t direction = 0;
Hooglugt 0:99cbc87af37c 83 uint8_t force = 0;
Hooglugt 0:99cbc87af37c 84
Hooglugt 0:99cbc87af37c 85 //motorcontrol objects
Hooglugt 0:99cbc87af37c 86
Hooglugt 0:99cbc87af37c 87 //motor 1, voltage pins op M2
Hooglugt 0:99cbc87af37c 88 Encoder motor1(PTD3, PTD5);
Hooglugt 0:99cbc87af37c 89 DigitalOut motor1dir(PTC9);
Hooglugt 0:99cbc87af37c 90 PwmOut pwm_motor1(PTC8);
Hooglugt 0:99cbc87af37c 91
Hooglugt 0:99cbc87af37c 92 //motor 2, voltage pins op M1
Hooglugt 0:99cbc87af37c 93 Encoder motor2(PTD2,PTD0);
Hooglugt 0:99cbc87af37c 94 DigitalOut motor2dir(PTA4);
Hooglugt 0:99cbc87af37c 95 PwmOut pwm_motor2(PTA5);
Hooglugt 0:99cbc87af37c 96
Hooglugt 0:99cbc87af37c 97 float integral = 0;
Hooglugt 7:ca1ade91bd14 98 float derivative = 0;
Hooglugt 4:697d5a806cc4 99 float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
Hooglugt 4:697d5a806cc4 100 float balhit = 0; //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
Hooglugt 7:ca1ade91bd14 101 float kalibratie = 0;
Hooglugt 0:99cbc87af37c 102 float controlerror = 0;
Hooglugt 6:14051758db6f 103 float previouserror = 0;
Hooglugt 0:99cbc87af37c 104 float pwm = 0;
Hooglugt 0:99cbc87af37c 105
Hooglugt 0:99cbc87af37c 106 float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
Hooglugt 1:d44a866de64f 107 float omrekenfactor2 = 0.0015213178; // 6.28/(24*172);
Hooglugt 0:99cbc87af37c 108
Hooglugt 1:d44a866de64f 109 float setpoint1 = 0; //te behalen speed van motor1 (37D)
Hooglugt 1:d44a866de64f 110 float setpoint2 = 0; //te behalen hoek van motor2 (25D)
Hooglugt 0:99cbc87af37c 111
Hooglugt 0:99cbc87af37c 112 float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
Hooglugt 0:99cbc87af37c 113 float Ki1 = 0.20; //Kp en Ki van motor1, voor de slag
Hooglugt 6:14051758db6f 114 float Kd1 = 0.0;
Hooglugt 6:14051758db6f 115
Hooglugt 7:ca1ade91bd14 116 float Kp3 = 0.15; //Kp en Ki van motor1, voor de return
Hooglugt 7:ca1ade91bd14 117 float Ki3 = 0.05; //0.09, 0.05
Hooglugt 7:ca1ade91bd14 118 float Kd3 = 0.1;
Hooglugt 0:99cbc87af37c 119
Hooglugt 7:ca1ade91bd14 120 float Kp2 = 1.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return
Hooglugt 7:ca1ade91bd14 121 float Ki2 = 0.8; //0.30 en 0.20
Hooglugt 7:ca1ade91bd14 122 float Kd2 = 0.1;
Hooglugt 0:99cbc87af37c 123
Hooglugt 7:ca1ade91bd14 124 float Kp4 = 1.0; //Kp en Ki van motor2, voor de return
Hooglugt 7:ca1ade91bd14 125 float Ki4 = 0.8;
Hooglugt 7:ca1ade91bd14 126 float Kd4 = 0.1;
Hooglugt 0:99cbc87af37c 127
Hooglugt 0:99cbc87af37c 128 volatile bool looptimerflag; //voor motorcontrol TSAMP
Hooglugt 0:99cbc87af37c 129
Hooglugt 0:99cbc87af37c 130 //functies
Hooglugt 0:99cbc87af37c 131
Hooglugt 0:99cbc87af37c 132 void setlooptimerflag(void)
Hooglugt 0:99cbc87af37c 133 {
Hooglugt 0:99cbc87af37c 134 looptimerflag = true;
Hooglugt 7:ca1ade91bd14 135 scope.set(0, motor2.getPosition()*omrekenfactor2);
Hooglugt 7:ca1ade91bd14 136 scope.set(0, motor1.getPosition()*omrekenfactor1);
Hooglugt 7:ca1ade91bd14 137 scope.send();
Hooglugt 0:99cbc87af37c 138
Hooglugt 0:99cbc87af37c 139 }
Hooglugt 0:99cbc87af37c 140
Hooglugt 0:99cbc87af37c 141 void keep_in_range(float * in, float min, float max)
Hooglugt 0:99cbc87af37c 142 {
Hooglugt 0:99cbc87af37c 143 *in > min ? *in < max? : *in = max: *in = max;
Hooglugt 0:99cbc87af37c 144 }
Hooglugt 0:99cbc87af37c 145
Hooglugt 0:99cbc87af37c 146 void looper()
Hooglugt 0:99cbc87af37c 147 {
Hooglugt 0:99cbc87af37c 148 //put raw emg value of biceps and triceps in emg_biceps and emg_triceps, respectively
Hooglugt 0:99cbc87af37c 149 float emg_biceps; //Float voor EMG-waarde biceps
Hooglugt 0:99cbc87af37c 150 float emg_triceps; //Float voor EMG-waarde triceps
Hooglugt 0:99cbc87af37c 151
Hooglugt 0:99cbc87af37c 152 emg_biceps = emg0.read(); // read float value (0..1 = 0..3.3V) biceps
Hooglugt 0:99cbc87af37c 153 emg_triceps = emg1.read(); // read float value (0..1 = 0..3.3V) triceps
Hooglugt 0:99cbc87af37c 154
Hooglugt 0:99cbc87af37c 155 //process emg biceps
Hooglugt 0:99cbc87af37c 156 arm_biquad_cascade_df1_f32(&bihighpass, &emg_biceps, &emg_biceps, 1 );
Hooglugt 0:99cbc87af37c 157 arm_biquad_cascade_df1_f32(&binotch, &emg_biceps, &emg_biceps, 1 );
Hooglugt 0:99cbc87af37c 158 y0 = fabs(emg_biceps);
Hooglugt 0:99cbc87af37c 159 bi_result = (y0*0.1 +y1*0.1 + y2*0.1 + y3*0.1 + y4*0.1 + y5*0.1 + y6*0.1 + y7*0.1 + y8*0.1 + y9*0.1);
Hooglugt 0:99cbc87af37c 160 y9=y8;
Hooglugt 0:99cbc87af37c 161 y8=y7;
Hooglugt 0:99cbc87af37c 162 y7=y6;
Hooglugt 0:99cbc87af37c 163 y6=y5;
Hooglugt 0:99cbc87af37c 164 y5=y4;
Hooglugt 0:99cbc87af37c 165 y4=y3;
Hooglugt 0:99cbc87af37c 166 y3=y2;
Hooglugt 0:99cbc87af37c 167 y2=y1;
Hooglugt 0:99cbc87af37c 168 y1=y0;
Hooglugt 0:99cbc87af37c 169
Hooglugt 0:99cbc87af37c 170 //process emg triceps
Hooglugt 0:99cbc87af37c 171 arm_biquad_cascade_df1_f32(&trihighpass, &emg_triceps, &emg_triceps, 1 );
Hooglugt 0:99cbc87af37c 172 arm_biquad_cascade_df1_f32(&trinotch, &emg_triceps, &emg_triceps, 1 );
Hooglugt 0:99cbc87af37c 173 x0 = fabs(emg_triceps);
Hooglugt 0:99cbc87af37c 174 tri_result = (x0*0.1 +x1*0.1 + x2*0.1 + x3*0.1 + x4*0.1 + x5*0.1 + x6*0.1 + x7*0.1 + x8*0.1 + x9*0.1);
Hooglugt 0:99cbc87af37c 175 x9=x8;
Hooglugt 0:99cbc87af37c 176 x8=x7;
Hooglugt 0:99cbc87af37c 177 x7=x6;
Hooglugt 0:99cbc87af37c 178 x6=x5;
Hooglugt 0:99cbc87af37c 179 x5=x4;
Hooglugt 0:99cbc87af37c 180 x4=x3;
Hooglugt 0:99cbc87af37c 181 x3=x2;
Hooglugt 0:99cbc87af37c 182 x2=x1;
Hooglugt 0:99cbc87af37c 183 x1=x0;
Hooglugt 0:99cbc87af37c 184 }
Hooglugt 0:99cbc87af37c 185
Hooglugt 0:99cbc87af37c 186 void kalbi() //blinking three lights, first row - 2nd row unlit
Hooglugt 0:99cbc87af37c 187 {
Hooglugt 0:99cbc87af37c 188 if(dir1==0) {
Hooglugt 0:99cbc87af37c 189 dir1 = dir2 = dir3 = 1;
Hooglugt 0:99cbc87af37c 190 } else {
Hooglugt 0:99cbc87af37c 191 dir1 = dir2 = dir3 = 0;
Hooglugt 0:99cbc87af37c 192 }
Hooglugt 0:99cbc87af37c 193 }
Hooglugt 0:99cbc87af37c 194
Hooglugt 0:99cbc87af37c 195 void kaltri() //blinking three lights, 2nd row - first row lit
Hooglugt 0:99cbc87af37c 196 {
Hooglugt 0:99cbc87af37c 197 if(for1==0) {
Hooglugt 0:99cbc87af37c 198 for1 = for2 = for3 = 1;
Hooglugt 0:99cbc87af37c 199 } else {
Hooglugt 0:99cbc87af37c 200 for1 = for2 = for3 = 0;
Hooglugt 0:99cbc87af37c 201 }
Hooglugt 0:99cbc87af37c 202 }
Hooglugt 0:99cbc87af37c 203
Hooglugt 0:99cbc87af37c 204 void okay() //blinking the two lights you have chosen (misschien is hier een betere manier van coderen voor :P)
Hooglugt 0:99cbc87af37c 205 {
Hooglugt 0:99cbc87af37c 206 if(direction == 1 && force == 1) { // links zwak
Hooglugt 0:99cbc87af37c 207 if(for1 == 0 && dir1 == 0) {
Hooglugt 0:99cbc87af37c 208 for1 = dir1 = 1;
Hooglugt 0:99cbc87af37c 209 } else {
Hooglugt 0:99cbc87af37c 210 for1 = dir1 = 0;
Hooglugt 0:99cbc87af37c 211 }
Hooglugt 0:99cbc87af37c 212 }
Hooglugt 0:99cbc87af37c 213 if(direction == 1 && force == 2) { // links normaal
Hooglugt 0:99cbc87af37c 214 if(for2 == 0 && dir1 == 0) {
Hooglugt 0:99cbc87af37c 215 for2 = dir1 = 1;
Hooglugt 0:99cbc87af37c 216 } else {
Hooglugt 0:99cbc87af37c 217 for2 = dir1 = 0;
Hooglugt 0:99cbc87af37c 218 }
Hooglugt 0:99cbc87af37c 219 }
Hooglugt 0:99cbc87af37c 220 if(direction == 1 && force == 3) { // links sterk
Hooglugt 0:99cbc87af37c 221 if(for3 == 0 && dir1 == 0) {
Hooglugt 0:99cbc87af37c 222 for3 = dir1 = 1;
Hooglugt 0:99cbc87af37c 223 } else {
Hooglugt 0:99cbc87af37c 224 for3 = dir1 = 0;
Hooglugt 0:99cbc87af37c 225 }
Hooglugt 0:99cbc87af37c 226 }
Hooglugt 0:99cbc87af37c 227 if(direction == 2 && force == 1) { // mid zwak
Hooglugt 0:99cbc87af37c 228 if(for1 == 0 && dir2 == 0) {
Hooglugt 0:99cbc87af37c 229 for1 = dir2 = 1;
Hooglugt 0:99cbc87af37c 230 } else {
Hooglugt 0:99cbc87af37c 231 for1 = dir2 = 0;
Hooglugt 0:99cbc87af37c 232 }
Hooglugt 0:99cbc87af37c 233 }
Hooglugt 0:99cbc87af37c 234 if(direction == 2 && force == 2) { // mid normaal
Hooglugt 0:99cbc87af37c 235 if(for2 == 0 && dir2 == 0) {
Hooglugt 0:99cbc87af37c 236 for2 = dir2 = 1;
Hooglugt 0:99cbc87af37c 237 } else {
Hooglugt 0:99cbc87af37c 238 for2 = dir2 = 0;
Hooglugt 0:99cbc87af37c 239 }
Hooglugt 0:99cbc87af37c 240 }
Hooglugt 0:99cbc87af37c 241 if(direction == 2 && force == 3) { // mid sterk
Hooglugt 0:99cbc87af37c 242 if(for3 == 0 && dir2 == 0) {
Hooglugt 0:99cbc87af37c 243 for3 = dir2 = 1;
Hooglugt 0:99cbc87af37c 244 } else {
Hooglugt 0:99cbc87af37c 245 for3 = dir2 = 0;
Hooglugt 0:99cbc87af37c 246 }
Hooglugt 0:99cbc87af37c 247 }
Hooglugt 0:99cbc87af37c 248 if(direction == 3 && force == 1) { // rechts zwak
Hooglugt 0:99cbc87af37c 249 if(for1 == 0 && dir3 == 0) {
Hooglugt 0:99cbc87af37c 250 for1 = dir3 = 1;
Hooglugt 0:99cbc87af37c 251 } else {
Hooglugt 0:99cbc87af37c 252 for1 = dir3 = 0;
Hooglugt 0:99cbc87af37c 253 }
Hooglugt 0:99cbc87af37c 254 }
Hooglugt 0:99cbc87af37c 255 if(direction == 3 && force == 2) { // rechts normaal
Hooglugt 0:99cbc87af37c 256 if(for2 == 0 && dir3 == 0) {
Hooglugt 0:99cbc87af37c 257 for2 = dir3 = 1;
Hooglugt 0:99cbc87af37c 258 } else {
Hooglugt 0:99cbc87af37c 259 for2 = dir3 = 0;
Hooglugt 0:99cbc87af37c 260 }
Hooglugt 0:99cbc87af37c 261 }
Hooglugt 0:99cbc87af37c 262 if(direction == 3 && force == 3) { // rechts sterk
Hooglugt 0:99cbc87af37c 263 if(for3 == 0 && dir3 == 0) {
Hooglugt 0:99cbc87af37c 264 for3 = dir3 = 1;
Hooglugt 0:99cbc87af37c 265 } else {
Hooglugt 0:99cbc87af37c 266 for3 = dir3 = 0;
Hooglugt 0:99cbc87af37c 267 }
Hooglugt 0:99cbc87af37c 268 }
Hooglugt 0:99cbc87af37c 269 }
Hooglugt 0:99cbc87af37c 270
Hooglugt 0:99cbc87af37c 271 int main()
Hooglugt 0:99cbc87af37c 272 {
Hooglugt 0:99cbc87af37c 273 pc.baud(115200); //baudrate instellen
Hooglugt 0:99cbc87af37c 274 log_timer.attach(looper, TSAMP_EMG); //EMG, Fsample 500 Hz
Hooglugt 0:99cbc87af37c 275 looptimer.attach(setlooptimerflag,TSAMP);
Hooglugt 0:99cbc87af37c 276 pwm_motor1.period_us(100); //10kHz PWM frequency
Hooglugt 0:99cbc87af37c 277 pwm_motor2.period_us(100); //10kHz PWM frequency
Hooglugt 0:99cbc87af37c 278
Hooglugt 0:99cbc87af37c 279 //set up filters
Hooglugt 0:99cbc87af37c 280 arm_biquad_cascade_df1_init_f32(&binotch, 1, binotch_const, binotch_states);
Hooglugt 0:99cbc87af37c 281 arm_biquad_cascade_df1_init_f32(&bihighpass, 1, bihighpass_const, bihighpass_states);
Hooglugt 0:99cbc87af37c 282
Hooglugt 0:99cbc87af37c 283 arm_biquad_cascade_df1_init_f32(&trinotch, 1, trinotch_const, trinotch_states);
Hooglugt 0:99cbc87af37c 284 arm_biquad_cascade_df1_init_f32(&trihighpass, 1, trihighpass_const, trihighpass_states);
Hooglugt 0:99cbc87af37c 285
Hooglugt 0:99cbc87af37c 286 //kalibratie
Hooglugt 0:99cbc87af37c 287
Hooglugt 0:99cbc87af37c 288 //motorarm naar nul-positie
Hooglugt 0:99cbc87af37c 289 blink.attach(kalbi, 0.2);
Hooglugt 1:d44a866de64f 290 blink2.attach(kaltri, 0.2);
Hooglugt 0:99cbc87af37c 291
Hooglugt 0:99cbc87af37c 292 //calibration motor 2
Hooglugt 7:ca1ade91bd14 293 pwm_motor2.write(0.65); //lage PWM
Hooglugt 5:e5ca53305b87 294 motor2dir = 0; //rechtsom
Hooglugt 0:99cbc87af37c 295 wait(1); // anders wordt de while(1) meteen onderbroken
Hooglugt 4:697d5a806cc4 296 while(1) {
Hooglugt 5:e5ca53305b87 297 if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // motor2.getSpeed()*omrekenfactor2 > -0.70), 0.70 is nog aan te passen
Hooglugt 0:99cbc87af37c 298 pwm_motor2.write(0);
Hooglugt 4:697d5a806cc4 299 motor2.setPosition(0);
Hooglugt 4:697d5a806cc4 300 goto motor1cal;
Hooglugt 0:99cbc87af37c 301 }
Hooglugt 0:99cbc87af37c 302 wait(0.01);
Hooglugt 0:99cbc87af37c 303 }
Hooglugt 4:697d5a806cc4 304 motor1cal:
Hooglugt 0:99cbc87af37c 305 //calibration motor 1
Hooglugt 7:ca1ade91bd14 306 pwm_motor1.write(0.65); //lage PWM
Hooglugt 7:ca1ade91bd14 307 motor1dir = 0; //linksom
Hooglugt 0:99cbc87af37c 308 wait(1); // anders wordt de while(1) meteen onderbroken
Hooglugt 4:697d5a806cc4 309 while(1) {
Hooglugt 5:e5ca53305b87 310 if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // motor1.getSpeed()*omrekenfactor1 < 0.20, 0.20 is nog aan te passen
Hooglugt 0:99cbc87af37c 311 pwm_motor1.write(0);
Hooglugt 4:697d5a806cc4 312 motor1.setPosition(0);
Hooglugt 4:697d5a806cc4 313 goto emgcal;
Hooglugt 0:99cbc87af37c 314 }
Hooglugt 0:99cbc87af37c 315 wait(0.01);
Hooglugt 0:99cbc87af37c 316 }
Hooglugt 4:697d5a806cc4 317 emgcal:
Hooglugt 0:99cbc87af37c 318 blink.detach();
Hooglugt 1:d44a866de64f 319 blink2.detach();
Hooglugt 0:99cbc87af37c 320 dir1 = dir2 = dir3 = 1;
Hooglugt 4:697d5a806cc4 321 for1 = for2 = for3 = 1;
Hooglugt 4:697d5a806cc4 322 pc.printf("kalmoarm ");
Hooglugt 0:99cbc87af37c 323 wait (1);
Hooglugt 0:99cbc87af37c 324 for1 = for2 = for3 = 0;
Hooglugt 0:99cbc87af37c 325
Hooglugt 7:ca1ade91bd14 326 if(kalibratie==0) {
Hooglugt 0:99cbc87af37c 327 //biceps kalibratie
Hooglugt 0:99cbc87af37c 328 blink.attach(kalbi, 0.2);
Hooglugt 0:99cbc87af37c 329 for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
Hooglugt 0:99cbc87af37c 330 if (bi_max < bi_result) {
Hooglugt 0:99cbc87af37c 331 bi_max = bi_result;
Hooglugt 0:99cbc87af37c 332 }
Hooglugt 0:99cbc87af37c 333 wait (0.01);
Hooglugt 1:d44a866de64f 334 }
Hooglugt 1:d44a866de64f 335 blink.detach();
Hooglugt 1:d44a866de64f 336 dir1 = dir2 = dir3 = 1;
Hooglugt 4:697d5a806cc4 337 pc.printf("kalbi ");
Hooglugt 1:d44a866de64f 338 wait (1);
Hooglugt 0:99cbc87af37c 339
Hooglugt 1:d44a866de64f 340 //triceps kalibratie
Hooglugt 1:d44a866de64f 341 blink.attach(kaltri, 0.2);
Hooglugt 1:d44a866de64f 342 for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
Hooglugt 1:d44a866de64f 343 if (tri_max < tri_result) {
Hooglugt 1:d44a866de64f 344 tri_max = tri_result;
Hooglugt 0:99cbc87af37c 345 }
Hooglugt 1:d44a866de64f 346 wait (0.01);
Hooglugt 0:99cbc87af37c 347 }
Hooglugt 1:d44a866de64f 348 blink.detach();
Hooglugt 1:d44a866de64f 349 for1 = for2 = for3 = 1;
Hooglugt 4:697d5a806cc4 350 pc.printf("kaltri ");
Hooglugt 1:d44a866de64f 351 wait (1);
Hooglugt 1:d44a866de64f 352 for1 = for2 = for3 = 0;
Hooglugt 7:ca1ade91bd14 353 kalibratie = 1;
Hooglugt 7:ca1ade91bd14 354 }
Hooglugt 0:99cbc87af37c 355
Hooglugt 4:697d5a806cc4 356 directionchoice:
Hooglugt 1:d44a866de64f 357 log_timer.attach(looper, TSAMP_EMG);
Hooglugt 0:99cbc87af37c 358
Hooglugt 4:697d5a806cc4 359 while(1) { //Loop keuze DIRECTION
Hooglugt 0:99cbc87af37c 360 for(int i=1; i<4; i++) {
Hooglugt 0:99cbc87af37c 361 if(i==1) { //red
Hooglugt 0:99cbc87af37c 362 dir1=1;
Hooglugt 0:99cbc87af37c 363 dir2=0;
Hooglugt 0:99cbc87af37c 364 dir3=0;
Hooglugt 0:99cbc87af37c 365 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 366 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 367 direction = 1;
Hooglugt 4:697d5a806cc4 368 pc.printf("links ");
Hooglugt 0:99cbc87af37c 369 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 370 goto forcechoice; // goes to second while(1) for the deciding the force
Hooglugt 0:99cbc87af37c 371 } else {
Hooglugt 0:99cbc87af37c 372 wait(0.01);
Hooglugt 0:99cbc87af37c 373 }
Hooglugt 0:99cbc87af37c 374 }
Hooglugt 0:99cbc87af37c 375 }
Hooglugt 0:99cbc87af37c 376 if(i==2) { //green
Hooglugt 0:99cbc87af37c 377 dir1 =0;
Hooglugt 0:99cbc87af37c 378 dir2 =1;
Hooglugt 0:99cbc87af37c 379 dir3 =0;
Hooglugt 0:99cbc87af37c 380 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 381 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 382 direction = 2;
Hooglugt 4:697d5a806cc4 383 pc.printf("mid ");
Hooglugt 0:99cbc87af37c 384 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 385 goto forcechoice;
Hooglugt 0:99cbc87af37c 386 } else {
Hooglugt 0:99cbc87af37c 387 wait(0.01);
Hooglugt 0:99cbc87af37c 388 }
Hooglugt 0:99cbc87af37c 389 }
Hooglugt 0:99cbc87af37c 390 }
Hooglugt 0:99cbc87af37c 391 if(i==3) { //blue
Hooglugt 0:99cbc87af37c 392 dir1 =0;
Hooglugt 0:99cbc87af37c 393 dir2 =0;
Hooglugt 0:99cbc87af37c 394 dir3 =1;
Hooglugt 0:99cbc87af37c 395 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 396 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 397 direction = 3;
Hooglugt 4:697d5a806cc4 398 pc.printf("rechts ");
Hooglugt 0:99cbc87af37c 399 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 400 goto forcechoice;
Hooglugt 0:99cbc87af37c 401 } else {
Hooglugt 0:99cbc87af37c 402 wait(0.01);
Hooglugt 0:99cbc87af37c 403 }
Hooglugt 0:99cbc87af37c 404 }
Hooglugt 0:99cbc87af37c 405 }
Hooglugt 0:99cbc87af37c 406 }
Hooglugt 0:99cbc87af37c 407 }
Hooglugt 4:697d5a806cc4 408 forcechoice:
Hooglugt 4:697d5a806cc4 409 while(1) { //Loop keuze FORCE
Hooglugt 0:99cbc87af37c 410 for(int j=1; j<4; j++) {
Hooglugt 0:99cbc87af37c 411 if(j==1) { //red
Hooglugt 0:99cbc87af37c 412 for1=1;
Hooglugt 0:99cbc87af37c 413 for2=0;
Hooglugt 0:99cbc87af37c 414 for3=0;
Hooglugt 0:99cbc87af37c 415 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 416 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 417 for1 = for2 = for3 = 0;
Hooglugt 4:697d5a806cc4 418 pc.printf("reset ");
Hooglugt 0:99cbc87af37c 419 goto directionchoice;
Hooglugt 0:99cbc87af37c 420 } else {
Hooglugt 1:d44a866de64f 421 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 422 force = 1;
Hooglugt 4:697d5a806cc4 423 pc.printf("zwak ");
Hooglugt 0:99cbc87af37c 424 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 425 goto choicesmade;
Hooglugt 0:99cbc87af37c 426 } else {
Hooglugt 0:99cbc87af37c 427 wait(0.01);
Hooglugt 0:99cbc87af37c 428 }
Hooglugt 0:99cbc87af37c 429 }
Hooglugt 0:99cbc87af37c 430 }
Hooglugt 0:99cbc87af37c 431 }
Hooglugt 0:99cbc87af37c 432 if(j==2) { //green
Hooglugt 0:99cbc87af37c 433 for1=0;
Hooglugt 0:99cbc87af37c 434 for2=1;
Hooglugt 0:99cbc87af37c 435 for3=0;
Hooglugt 0:99cbc87af37c 436 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 437 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 438 for1 = for2 = for3 = 0;
Hooglugt 4:697d5a806cc4 439 pc.printf("reset ");
Hooglugt 0:99cbc87af37c 440 goto directionchoice;
Hooglugt 0:99cbc87af37c 441 } else {
Hooglugt 1:d44a866de64f 442 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 443 force = 2;
Hooglugt 4:697d5a806cc4 444 pc.printf("normaal ");
Hooglugt 0:99cbc87af37c 445 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 446 goto choicesmade;
Hooglugt 0:99cbc87af37c 447 } else {
Hooglugt 0:99cbc87af37c 448 wait(0.01);
Hooglugt 0:99cbc87af37c 449 }
Hooglugt 0:99cbc87af37c 450 }
Hooglugt 0:99cbc87af37c 451 }
Hooglugt 0:99cbc87af37c 452 }
Hooglugt 0:99cbc87af37c 453 if(j==3) { //blue
Hooglugt 0:99cbc87af37c 454 for1=0;
Hooglugt 0:99cbc87af37c 455 for2=0;
Hooglugt 0:99cbc87af37c 456 for3=1;
Hooglugt 0:99cbc87af37c 457 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 458 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 459 for1 = for2 = for3 = 0;
Hooglugt 4:697d5a806cc4 460 pc.printf("reset ");
Hooglugt 0:99cbc87af37c 461 goto directionchoice;
Hooglugt 0:99cbc87af37c 462 } else {
Hooglugt 1:d44a866de64f 463 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 464 force = 3;
Hooglugt 4:697d5a806cc4 465 pc.printf("sterk ");
Hooglugt 0:99cbc87af37c 466 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 467 goto choicesmade;
Hooglugt 0:99cbc87af37c 468 } else {
Hooglugt 0:99cbc87af37c 469 wait(0.01);
Hooglugt 0:99cbc87af37c 470 }
Hooglugt 0:99cbc87af37c 471 }
Hooglugt 0:99cbc87af37c 472 }
Hooglugt 0:99cbc87af37c 473 }
Hooglugt 0:99cbc87af37c 474 }
Hooglugt 0:99cbc87af37c 475 }
Hooglugt 0:99cbc87af37c 476
Hooglugt 0:99cbc87af37c 477 choicesmade:
Hooglugt 0:99cbc87af37c 478 blink.attach(okay, 0.2);
Hooglugt 4:697d5a806cc4 479 while(1) {
Hooglugt 4:697d5a806cc4 480 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 481 blink.detach();
Hooglugt 4:697d5a806cc4 482 pc.printf("reset ");
Hooglugt 1:d44a866de64f 483 switch (direction) {
Hooglugt 1:d44a866de64f 484 case 1:
Hooglugt 1:d44a866de64f 485 dir1 = 1;
Hooglugt 1:d44a866de64f 486 for1 = 1;
Hooglugt 1:d44a866de64f 487 for2 = for3 = 0;
Hooglugt 1:d44a866de64f 488 break;
Hooglugt 1:d44a866de64f 489 case 2:
Hooglugt 1:d44a866de64f 490 dir2 = 1;
Hooglugt 1:d44a866de64f 491 for1 = 1;
Hooglugt 1:d44a866de64f 492 for2 = for3 = 0;
Hooglugt 1:d44a866de64f 493 break;
Hooglugt 1:d44a866de64f 494 case 3:
Hooglugt 1:d44a866de64f 495 dir3 = 1;
Hooglugt 1:d44a866de64f 496 for1 = 1;
Hooglugt 1:d44a866de64f 497 for2 = for3 = 0;
Hooglugt 1:d44a866de64f 498 break;
Hooglugt 1:d44a866de64f 499 }
Hooglugt 4:697d5a806cc4 500
Hooglugt 4:697d5a806cc4 501 wait(1); // 1 sec wait, anders reset je meteen ook de biceps keuze
Hooglugt 4:697d5a806cc4 502 goto forcechoice;
Hooglugt 0:99cbc87af37c 503 } else {
Hooglugt 1:d44a866de64f 504 if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) {
Hooglugt 4:697d5a806cc4 505 blink.detach();
Hooglugt 4:697d5a806cc4 506 log_timer.detach();
Hooglugt 4:697d5a806cc4 507 goto motorcontrol;
Hooglugt 0:99cbc87af37c 508 } else {
Hooglugt 0:99cbc87af37c 509 wait(0.01); // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje)
Hooglugt 0:99cbc87af37c 510 }
Hooglugt 0:99cbc87af37c 511 }
Hooglugt 0:99cbc87af37c 512 }
Hooglugt 4:697d5a806cc4 513
Hooglugt 4:697d5a806cc4 514 motorcontrol:
Hooglugt 4:697d5a806cc4 515
Hooglugt 0:99cbc87af37c 516 /* Vanaf hier komt de aansturing van de motor */
Hooglugt 0:99cbc87af37c 517
Hooglugt 1:d44a866de64f 518 switch (direction) {
Hooglugt 1:d44a866de64f 519 case 1:
Hooglugt 7:ca1ade91bd14 520 setpoint2 = (0.436332313+0.197222205); //25 graden + 11,3 graden, slag naar linkerdoel
Hooglugt 1:d44a866de64f 521 break;
Hooglugt 1:d44a866de64f 522 case 2:
Hooglugt 7:ca1ade91bd14 523 setpoint2 = (0.436332313); //25 graden vanaf nul-punt (precies midden)
Hooglugt 1:d44a866de64f 524 break;
Hooglugt 1:d44a866de64f 525 case 3:
Hooglugt 7:ca1ade91bd14 526 setpoint2 = (0.436332313-0.197222205); // 25 graden - 11,3 graden, slag naar rechterdoel
Hooglugt 1:d44a866de64f 527 break;
Hooglugt 0:99cbc87af37c 528 }
Hooglugt 1:d44a866de64f 529
Hooglugt 1:d44a866de64f 530 switch (force) {
Hooglugt 1:d44a866de64f 531 case 1:
Hooglugt 5:e5ca53305b87 532 setpoint1 = 6.8;
Hooglugt 1:d44a866de64f 533 break;
Hooglugt 1:d44a866de64f 534 case 2:
Hooglugt 5:e5ca53305b87 535 setpoint1 = 7.4;
Hooglugt 1:d44a866de64f 536 break;
Hooglugt 1:d44a866de64f 537 case 3:
Hooglugt 5:e5ca53305b87 538 setpoint1 = 8.0;
Hooglugt 1:d44a866de64f 539 break;
Hooglugt 0:99cbc87af37c 540 }
Hooglugt 0:99cbc87af37c 541
Hooglugt 4:697d5a806cc4 542 while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
Hooglugt 0:99cbc87af37c 543 while(!looptimerflag);
Hooglugt 0:99cbc87af37c 544 looptimerflag = false; //clear flag
Hooglugt 0:99cbc87af37c 545
Hooglugt 0:99cbc87af37c 546 //regelaar motor2, bepaalt positie
Hooglugt 0:99cbc87af37c 547 controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;
Hooglugt 0:99cbc87af37c 548 integral = integral + controlerror*TSAMP;
Hooglugt 6:14051758db6f 549 derivative = (controlerror - previouserror)/TSAMP;
Hooglugt 6:14051758db6f 550 pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative;
Hooglugt 6:14051758db6f 551 previouserror = controlerror;
Hooglugt 0:99cbc87af37c 552
Hooglugt 7:ca1ade91bd14 553 keep_in_range(&pwm, -1,1);
Hooglugt 7:ca1ade91bd14 554 pwm_motor2.write(abs(pwm));
Hooglugt 0:99cbc87af37c 555 if(pwm > 0) {
Hooglugt 0:99cbc87af37c 556 motor2dir = 1;
Hooglugt 0:99cbc87af37c 557 } else {
Hooglugt 0:99cbc87af37c 558 motor2dir = 0;
Hooglugt 0:99cbc87af37c 559 }
Hooglugt 0:99cbc87af37c 560
Hooglugt 7:ca1ade91bd14 561
Hooglugt 4:697d5a806cc4 562 //controleert of batje positie heeft bepaald
Hooglugt 4:697d5a806cc4 563 if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
Hooglugt 7:ca1ade91bd14 564 if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) {
Hooglugt 4:697d5a806cc4 565 batjeset = 0;
Hooglugt 4:697d5a806cc4 566 } else {
Hooglugt 4:697d5a806cc4 567 batjeset++;
Hooglugt 4:697d5a806cc4 568 }
Hooglugt 0:99cbc87af37c 569 } else {
Hooglugt 4:697d5a806cc4 570 pwm_motor2.write(0);
Hooglugt 6:14051758db6f 571 batjeset = integral = derivative = previouserror = 0;
Hooglugt 4:697d5a806cc4 572 wait(1);
Hooglugt 4:697d5a806cc4 573 goto motor1control;
Hooglugt 0:99cbc87af37c 574 }
Hooglugt 0:99cbc87af37c 575 }
Hooglugt 4:697d5a806cc4 576
Hooglugt 4:697d5a806cc4 577 motor1control:
Hooglugt 4:697d5a806cc4 578 while(1) { // loop voor het slaan mbv motor1 (batje snelheid)
Hooglugt 0:99cbc87af37c 579 while(!looptimerflag);
Hooglugt 0:99cbc87af37c 580 looptimerflag = false; //clear flag
Hooglugt 0:99cbc87af37c 581
Hooglugt 4:697d5a806cc4 582 if (balhit == 0) { //regelaar motor1, bepaalt snelheid
Hooglugt 4:697d5a806cc4 583 controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
Hooglugt 4:697d5a806cc4 584 integral = integral + controlerror*TSAMP;
Hooglugt 6:14051758db6f 585 derivative = (controlerror - previouserror)/TSAMP;
Hooglugt 6:14051758db6f 586 pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative;
Hooglugt 6:14051758db6f 587 previouserror = controlerror;
Hooglugt 4:697d5a806cc4 588 } else { //regelaar motor1, bepaalt positie
Hooglugt 6:14051758db6f 589 balhit = integral = derivative = previouserror = 0;
Hooglugt 4:697d5a806cc4 590 goto resetpositionmotor1;
Hooglugt 4:697d5a806cc4 591 }
Hooglugt 4:697d5a806cc4 592
Hooglugt 0:99cbc87af37c 593 keep_in_range(&pwm, -1,1);
Hooglugt 0:99cbc87af37c 594 pwm_motor1.write(abs(pwm));
Hooglugt 0:99cbc87af37c 595
Hooglugt 0:99cbc87af37c 596 if(pwm > 0) {
Hooglugt 0:99cbc87af37c 597 motor1dir = 1;
Hooglugt 0:99cbc87af37c 598 } else {
Hooglugt 0:99cbc87af37c 599 motor1dir = 0;
Hooglugt 0:99cbc87af37c 600 }
Hooglugt 0:99cbc87af37c 601
Hooglugt 0:99cbc87af37c 602 //controleert of batje balletje heeft bereikt
Hooglugt 0:99cbc87af37c 603 //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
Hooglugt 7:ca1ade91bd14 604 if (motor1.getPosition()*omrekenfactor1 > 1.60) {
Hooglugt 7:ca1ade91bd14 605 balhit = 1;
Hooglugt 0:99cbc87af37c 606 }
Hooglugt 0:99cbc87af37c 607 }
Hooglugt 4:697d5a806cc4 608
Hooglugt 4:697d5a806cc4 609 resetpositionmotor1:
Hooglugt 4:697d5a806cc4 610 while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst
Hooglugt 0:99cbc87af37c 611 while(!looptimerflag);
Hooglugt 0:99cbc87af37c 612 looptimerflag = false; //clear flag
Hooglugt 0:99cbc87af37c 613
Hooglugt 0:99cbc87af37c 614 //regelaar motor1, bepaalt positie
Hooglugt 0:99cbc87af37c 615 controlerror = -1*motor1.getPosition()*omrekenfactor1;
Hooglugt 0:99cbc87af37c 616 integral = integral + controlerror*TSAMP;
Hooglugt 6:14051758db6f 617 derivative = (controlerror - previouserror)/TSAMP;
Hooglugt 6:14051758db6f 618 pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative;
Hooglugt 6:14051758db6f 619 previouserror = controlerror;
Hooglugt 0:99cbc87af37c 620
Hooglugt 0:99cbc87af37c 621 keep_in_range(&pwm, -1,1);
Hooglugt 1:d44a866de64f 622 if(pwm > 0) {
Hooglugt 0:99cbc87af37c 623 motor1dir = 1;
Hooglugt 0:99cbc87af37c 624 } else {
Hooglugt 0:99cbc87af37c 625 motor1dir = 0; //1 = rechtsom, 0 = linksom
Hooglugt 0:99cbc87af37c 626 }
Hooglugt 0:99cbc87af37c 627
Hooglugt 0:99cbc87af37c 628 pwm_motor1.write(abs(pwm));
Hooglugt 0:99cbc87af37c 629
Hooglugt 0:99cbc87af37c 630 //controleert of arm terug in positie is
Hooglugt 4:697d5a806cc4 631 if(batjeset < 200) {
Hooglugt 7:ca1ade91bd14 632 if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) {
Hooglugt 4:697d5a806cc4 633 batjeset = 0;
Hooglugt 4:697d5a806cc4 634 } else {
Hooglugt 4:697d5a806cc4 635 batjeset++;
Hooglugt 4:697d5a806cc4 636 }
Hooglugt 0:99cbc87af37c 637 } else {
Hooglugt 4:697d5a806cc4 638 pwm_motor1.write(0);
Hooglugt 6:14051758db6f 639 batjeset = integral = derivative = previouserror = 0;
Hooglugt 4:697d5a806cc4 640 wait(1);
Hooglugt 4:697d5a806cc4 641 goto resetpositionmotor2;
Hooglugt 0:99cbc87af37c 642 }
Hooglugt 0:99cbc87af37c 643 }
Hooglugt 4:697d5a806cc4 644
Hooglugt 4:697d5a806cc4 645 resetpositionmotor2:
Hooglugt 4:697d5a806cc4 646 while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
Hooglugt 0:99cbc87af37c 647 while(!looptimerflag);
Hooglugt 0:99cbc87af37c 648 looptimerflag = false; //clear flag
Hooglugt 0:99cbc87af37c 649
Hooglugt 0:99cbc87af37c 650 //regelaar motor2, bepaalt positie
Hooglugt 0:99cbc87af37c 651 controlerror = -1*motor2.getPosition()*omrekenfactor2;
Hooglugt 0:99cbc87af37c 652 integral = integral + controlerror*TSAMP;
Hooglugt 6:14051758db6f 653 derivative = (controlerror - previouserror)/TSAMP;
Hooglugt 6:14051758db6f 654 pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative;
Hooglugt 6:14051758db6f 655 previouserror = controlerror;
Hooglugt 0:99cbc87af37c 656
Hooglugt 0:99cbc87af37c 657 keep_in_range(&pwm, -1,1);
Hooglugt 0:99cbc87af37c 658
Hooglugt 0:99cbc87af37c 659 if(pwm > 0) {
Hooglugt 0:99cbc87af37c 660 motor2dir = 1;
Hooglugt 0:99cbc87af37c 661 } else {
Hooglugt 0:99cbc87af37c 662 motor2dir = 0;
Hooglugt 0:99cbc87af37c 663 }
Hooglugt 0:99cbc87af37c 664
Hooglugt 0:99cbc87af37c 665 pwm_motor2.write(abs(pwm));
Hooglugt 0:99cbc87af37c 666
Hooglugt 0:99cbc87af37c 667 //controleert of batje positie heeft bepaald
Hooglugt 4:697d5a806cc4 668 if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
Hooglugt 7:ca1ade91bd14 669 if (motor2.getPosition()*omrekenfactor2 > 0.1 || motor2.getPosition()*omrekenfactor2 < -0.1) {
Hooglugt 4:697d5a806cc4 670 batjeset = 0;
Hooglugt 4:697d5a806cc4 671 } else {
Hooglugt 4:697d5a806cc4 672 batjeset++;
Hooglugt 4:697d5a806cc4 673 }
Hooglugt 0:99cbc87af37c 674 } else {
Hooglugt 4:697d5a806cc4 675 pwm_motor2.write(0);
Hooglugt 6:14051758db6f 676 batjeset = integral = derivative = previouserror = 0;
Hooglugt 4:697d5a806cc4 677 wait(1);
Hooglugt 4:697d5a806cc4 678 direction = force = 0;
Hooglugt 7:ca1ade91bd14 679 goto motor1cal;
Hooglugt 4:697d5a806cc4 680 }
Hooglugt 0:99cbc87af37c 681 }
Hooglugt 0:99cbc87af37c 682 } // end main