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@22:77d7065cc431, 2014-11-04 (annotated)

Committer:
Hooglugt
Date:
Tue Nov 04 10:27:56 2014 +0000
Revision:
22:77d7065cc431
Parent:
21:779de788eb92
Child:
23:913bf8a6f7d8
motor2 direct aanvoeren door een pwm erop te writen

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 16:a0a39512bd47 16 HIDScope scope(6);
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 18:5467bcc5cbf5 101 float kalibratie = 1; //NOG AAN TE PASSEN NAAR 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 8:75980dc35763 106 float pwm1 =0;
Hooglugt 8:75980dc35763 107 float integral1 = 0;
Hooglugt 8:75980dc35763 108 float derivative1 = 0;
Hooglugt 8:75980dc35763 109 float controlerror1 = 0;
Hooglugt 8:75980dc35763 110 float previouserror1 = 0;
Hooglugt 8:75980dc35763 111
Hooglugt 8:75980dc35763 112 int state = 1;
Hooglugt 9:0bc7f83b761e 113 int count = 0;
Hooglugt 9:0bc7f83b761e 114 float angle = 0;
Hooglugt 8:75980dc35763 115
Hooglugt 0:99cbc87af37c 116 float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
Hooglugt 8:75980dc35763 117 float omrekenfactor2 = 0.0015213178; // 6.28/(24*172);
Hooglugt 0:99cbc87af37c 118
Hooglugt 1:d44a866de64f 119 float setpoint1 = 0; //te behalen speed van motor1 (37D)
Hooglugt 1:d44a866de64f 120 float setpoint2 = 0; //te behalen hoek van motor2 (25D)
Hooglugt 0:99cbc87af37c 121
Hooglugt 18:5467bcc5cbf5 122 float Kp1 = 2.0; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
Hooglugt 9:0bc7f83b761e 123 float Ki1 = 0.0; //Kp en Ki van motor1, voor de slag
Hooglugt 6:14051758db6f 124 float Kd1 = 0.0;
Hooglugt 6:14051758db6f 125
Hooglugt 22:77d7065cc431 126 float Kp2 = 0.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return
Hooglugt 20:5007ddcd03ef 127 float Ki2 = 0.0; //0.30 en 0.20
Hooglugt 9:0bc7f83b761e 128 float Kd2 = 0.0;
Hooglugt 0:99cbc87af37c 129
Hooglugt 0:99cbc87af37c 130 volatile bool looptimerflag; //voor motorcontrol TSAMP
Hooglugt 0:99cbc87af37c 131
Hooglugt 0:99cbc87af37c 132 //functies
Hooglugt 0:99cbc87af37c 133
Hooglugt 0:99cbc87af37c 134 void setlooptimerflag(void)
Hooglugt 0:99cbc87af37c 135 {
Hooglugt 0:99cbc87af37c 136 looptimerflag = true;
Hooglugt 10:6bf3e25f020a 137 }
Hooglugt 10:6bf3e25f020a 138
Hooglugt 10:6bf3e25f020a 139 Ticker hid;
Hooglugt 10:6bf3e25f020a 140
Hooglugt 10:6bf3e25f020a 141 void hidscope(void){
Hooglugt 11:b517e73a98ab 142
Hooglugt 10:6bf3e25f020a 143 scope.send();
Hooglugt 0:99cbc87af37c 144 }
Hooglugt 0:99cbc87af37c 145
Hooglugt 0:99cbc87af37c 146 void keep_in_range(float * in, float min, float max)
Hooglugt 0:99cbc87af37c 147 {
Hooglugt 0:99cbc87af37c 148 *in > min ? *in < max? : *in = max: *in = max;
Hooglugt 0:99cbc87af37c 149 }
Hooglugt 0:99cbc87af37c 150
Hooglugt 0:99cbc87af37c 151 void looper()
Hooglugt 0:99cbc87af37c 152 {
Hooglugt 0:99cbc87af37c 153 //put raw emg value of biceps and triceps in emg_biceps and emg_triceps, respectively
Hooglugt 0:99cbc87af37c 154 float emg_biceps; //Float voor EMG-waarde biceps
Hooglugt 0:99cbc87af37c 155 float emg_triceps; //Float voor EMG-waarde triceps
Hooglugt 0:99cbc87af37c 156
Hooglugt 0:99cbc87af37c 157 emg_biceps = emg0.read(); // read float value (0..1 = 0..3.3V) biceps
Hooglugt 0:99cbc87af37c 158 emg_triceps = emg1.read(); // read float value (0..1 = 0..3.3V) triceps
Hooglugt 0:99cbc87af37c 159
Hooglugt 0:99cbc87af37c 160 //process emg biceps
Hooglugt 0:99cbc87af37c 161 arm_biquad_cascade_df1_f32(&bihighpass, &emg_biceps, &emg_biceps, 1 );
Hooglugt 0:99cbc87af37c 162 arm_biquad_cascade_df1_f32(&binotch, &emg_biceps, &emg_biceps, 1 );
Hooglugt 0:99cbc87af37c 163 y0 = fabs(emg_biceps);
Hooglugt 0:99cbc87af37c 164 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 165 y9=y8;
Hooglugt 0:99cbc87af37c 166 y8=y7;
Hooglugt 0:99cbc87af37c 167 y7=y6;
Hooglugt 0:99cbc87af37c 168 y6=y5;
Hooglugt 0:99cbc87af37c 169 y5=y4;
Hooglugt 0:99cbc87af37c 170 y4=y3;
Hooglugt 0:99cbc87af37c 171 y3=y2;
Hooglugt 0:99cbc87af37c 172 y2=y1;
Hooglugt 0:99cbc87af37c 173 y1=y0;
Hooglugt 0:99cbc87af37c 174
Hooglugt 0:99cbc87af37c 175 //process emg triceps
Hooglugt 0:99cbc87af37c 176 arm_biquad_cascade_df1_f32(&trihighpass, &emg_triceps, &emg_triceps, 1 );
Hooglugt 0:99cbc87af37c 177 arm_biquad_cascade_df1_f32(&trinotch, &emg_triceps, &emg_triceps, 1 );
Hooglugt 0:99cbc87af37c 178 x0 = fabs(emg_triceps);
Hooglugt 0:99cbc87af37c 179 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 180 x9=x8;
Hooglugt 0:99cbc87af37c 181 x8=x7;
Hooglugt 0:99cbc87af37c 182 x7=x6;
Hooglugt 0:99cbc87af37c 183 x6=x5;
Hooglugt 0:99cbc87af37c 184 x5=x4;
Hooglugt 0:99cbc87af37c 185 x4=x3;
Hooglugt 0:99cbc87af37c 186 x3=x2;
Hooglugt 0:99cbc87af37c 187 x2=x1;
Hooglugt 0:99cbc87af37c 188 x1=x0;
Hooglugt 0:99cbc87af37c 189 }
Hooglugt 0:99cbc87af37c 190
Hooglugt 0:99cbc87af37c 191 void kalbi() //blinking three lights, first row - 2nd row unlit
Hooglugt 0:99cbc87af37c 192 {
Hooglugt 0:99cbc87af37c 193 if(dir1==0) {
Hooglugt 0:99cbc87af37c 194 dir1 = dir2 = dir3 = 1;
Hooglugt 0:99cbc87af37c 195 } else {
Hooglugt 0:99cbc87af37c 196 dir1 = dir2 = dir3 = 0;
Hooglugt 0:99cbc87af37c 197 }
Hooglugt 0:99cbc87af37c 198 }
Hooglugt 0:99cbc87af37c 199
Hooglugt 0:99cbc87af37c 200 void kaltri() //blinking three lights, 2nd row - first row lit
Hooglugt 0:99cbc87af37c 201 {
Hooglugt 0:99cbc87af37c 202 if(for1==0) {
Hooglugt 0:99cbc87af37c 203 for1 = for2 = for3 = 1;
Hooglugt 0:99cbc87af37c 204 } else {
Hooglugt 0:99cbc87af37c 205 for1 = for2 = for3 = 0;
Hooglugt 0:99cbc87af37c 206 }
Hooglugt 0:99cbc87af37c 207 }
Hooglugt 0:99cbc87af37c 208
Hooglugt 0:99cbc87af37c 209 void okay() //blinking the two lights you have chosen (misschien is hier een betere manier van coderen voor :P)
Hooglugt 0:99cbc87af37c 210 {
Hooglugt 0:99cbc87af37c 211 if(direction == 1 && force == 1) { // links zwak
Hooglugt 0:99cbc87af37c 212 if(for1 == 0 && dir1 == 0) {
Hooglugt 0:99cbc87af37c 213 for1 = dir1 = 1;
Hooglugt 0:99cbc87af37c 214 } else {
Hooglugt 0:99cbc87af37c 215 for1 = dir1 = 0;
Hooglugt 0:99cbc87af37c 216 }
Hooglugt 0:99cbc87af37c 217 }
Hooglugt 0:99cbc87af37c 218 if(direction == 1 && force == 2) { // links normaal
Hooglugt 0:99cbc87af37c 219 if(for2 == 0 && dir1 == 0) {
Hooglugt 0:99cbc87af37c 220 for2 = dir1 = 1;
Hooglugt 0:99cbc87af37c 221 } else {
Hooglugt 0:99cbc87af37c 222 for2 = dir1 = 0;
Hooglugt 0:99cbc87af37c 223 }
Hooglugt 0:99cbc87af37c 224 }
Hooglugt 0:99cbc87af37c 225 if(direction == 1 && force == 3) { // links sterk
Hooglugt 0:99cbc87af37c 226 if(for3 == 0 && dir1 == 0) {
Hooglugt 0:99cbc87af37c 227 for3 = dir1 = 1;
Hooglugt 0:99cbc87af37c 228 } else {
Hooglugt 0:99cbc87af37c 229 for3 = dir1 = 0;
Hooglugt 0:99cbc87af37c 230 }
Hooglugt 0:99cbc87af37c 231 }
Hooglugt 0:99cbc87af37c 232 if(direction == 2 && force == 1) { // mid zwak
Hooglugt 0:99cbc87af37c 233 if(for1 == 0 && dir2 == 0) {
Hooglugt 0:99cbc87af37c 234 for1 = dir2 = 1;
Hooglugt 0:99cbc87af37c 235 } else {
Hooglugt 0:99cbc87af37c 236 for1 = dir2 = 0;
Hooglugt 0:99cbc87af37c 237 }
Hooglugt 0:99cbc87af37c 238 }
Hooglugt 0:99cbc87af37c 239 if(direction == 2 && force == 2) { // mid normaal
Hooglugt 0:99cbc87af37c 240 if(for2 == 0 && dir2 == 0) {
Hooglugt 0:99cbc87af37c 241 for2 = dir2 = 1;
Hooglugt 0:99cbc87af37c 242 } else {
Hooglugt 0:99cbc87af37c 243 for2 = dir2 = 0;
Hooglugt 0:99cbc87af37c 244 }
Hooglugt 0:99cbc87af37c 245 }
Hooglugt 0:99cbc87af37c 246 if(direction == 2 && force == 3) { // mid sterk
Hooglugt 0:99cbc87af37c 247 if(for3 == 0 && dir2 == 0) {
Hooglugt 0:99cbc87af37c 248 for3 = dir2 = 1;
Hooglugt 0:99cbc87af37c 249 } else {
Hooglugt 0:99cbc87af37c 250 for3 = dir2 = 0;
Hooglugt 0:99cbc87af37c 251 }
Hooglugt 0:99cbc87af37c 252 }
Hooglugt 0:99cbc87af37c 253 if(direction == 3 && force == 1) { // rechts zwak
Hooglugt 0:99cbc87af37c 254 if(for1 == 0 && dir3 == 0) {
Hooglugt 0:99cbc87af37c 255 for1 = dir3 = 1;
Hooglugt 0:99cbc87af37c 256 } else {
Hooglugt 0:99cbc87af37c 257 for1 = dir3 = 0;
Hooglugt 0:99cbc87af37c 258 }
Hooglugt 0:99cbc87af37c 259 }
Hooglugt 0:99cbc87af37c 260 if(direction == 3 && force == 2) { // rechts normaal
Hooglugt 0:99cbc87af37c 261 if(for2 == 0 && dir3 == 0) {
Hooglugt 0:99cbc87af37c 262 for2 = dir3 = 1;
Hooglugt 0:99cbc87af37c 263 } else {
Hooglugt 0:99cbc87af37c 264 for2 = dir3 = 0;
Hooglugt 0:99cbc87af37c 265 }
Hooglugt 0:99cbc87af37c 266 }
Hooglugt 0:99cbc87af37c 267 if(direction == 3 && force == 3) { // rechts sterk
Hooglugt 0:99cbc87af37c 268 if(for3 == 0 && dir3 == 0) {
Hooglugt 0:99cbc87af37c 269 for3 = dir3 = 1;
Hooglugt 0:99cbc87af37c 270 } else {
Hooglugt 0:99cbc87af37c 271 for3 = dir3 = 0;
Hooglugt 0:99cbc87af37c 272 }
Hooglugt 0:99cbc87af37c 273 }
Hooglugt 0:99cbc87af37c 274 }
Hooglugt 0:99cbc87af37c 275
Hooglugt 0:99cbc87af37c 276 int main()
Hooglugt 0:99cbc87af37c 277 {
Hooglugt 11:b517e73a98ab 278 hid.attach(hidscope, 0.01);
Hooglugt 0:99cbc87af37c 279 pc.baud(115200); //baudrate instellen
Hooglugt 0:99cbc87af37c 280 log_timer.attach(looper, TSAMP_EMG); //EMG, Fsample 500 Hz
Hooglugt 0:99cbc87af37c 281 looptimer.attach(setlooptimerflag,TSAMP);
Hooglugt 0:99cbc87af37c 282 pwm_motor1.period_us(100); //10kHz PWM frequency
Hooglugt 0:99cbc87af37c 283 pwm_motor2.period_us(100); //10kHz PWM frequency
Hooglugt 0:99cbc87af37c 284
Hooglugt 0:99cbc87af37c 285 //set up filters
Hooglugt 0:99cbc87af37c 286 arm_biquad_cascade_df1_init_f32(&binotch, 1, binotch_const, binotch_states);
Hooglugt 0:99cbc87af37c 287 arm_biquad_cascade_df1_init_f32(&bihighpass, 1, bihighpass_const, bihighpass_states);
Hooglugt 0:99cbc87af37c 288
Hooglugt 0:99cbc87af37c 289 arm_biquad_cascade_df1_init_f32(&trinotch, 1, trinotch_const, trinotch_states);
Hooglugt 0:99cbc87af37c 290 arm_biquad_cascade_df1_init_f32(&trihighpass, 1, trihighpass_const, trihighpass_states);
Hooglugt 0:99cbc87af37c 291
Hooglugt 0:99cbc87af37c 292 //kalibratie
Hooglugt 0:99cbc87af37c 293
Hooglugt 18:5467bcc5cbf5 294 motor2cal:
Hooglugt 18:5467bcc5cbf5 295
Hooglugt 0:99cbc87af37c 296 //motorarm naar nul-positie
Hooglugt 20:5007ddcd03ef 297 blink.attach(kalbi, 0.4);
Hooglugt 1:d44a866de64f 298 blink2.attach(kaltri, 0.2);
Hooglugt 0:99cbc87af37c 299
Hooglugt 0:99cbc87af37c 300 //calibration motor 2
Hooglugt 7:ca1ade91bd14 301 pwm_motor2.write(0.65); //lage PWM
Hooglugt 5:e5ca53305b87 302 motor2dir = 0; //rechtsom
Hooglugt 0:99cbc87af37c 303 wait(1); // anders wordt de while(1) meteen onderbroken
Hooglugt 4:697d5a806cc4 304 while(1) {
Hooglugt 5:e5ca53305b87 305 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 306 pwm_motor2.write(0);
Hooglugt 4:697d5a806cc4 307 motor2.setPosition(0);
Hooglugt 4:697d5a806cc4 308 goto motor1cal;
Hooglugt 0:99cbc87af37c 309 }
Hooglugt 0:99cbc87af37c 310 wait(0.01);
Hooglugt 0:99cbc87af37c 311 }
Hooglugt 4:697d5a806cc4 312 motor1cal:
Hooglugt 0:99cbc87af37c 313 //calibration motor 1
Hooglugt 7:ca1ade91bd14 314 pwm_motor1.write(0.65); //lage PWM
Hooglugt 7:ca1ade91bd14 315 motor1dir = 0; //linksom
Hooglugt 0:99cbc87af37c 316 wait(1); // anders wordt de while(1) meteen onderbroken
Hooglugt 4:697d5a806cc4 317 while(1) {
Hooglugt 8:75980dc35763 318 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 319 pwm_motor1.write(0);
Hooglugt 4:697d5a806cc4 320 motor1.setPosition(0);
Hooglugt 4:697d5a806cc4 321 goto emgcal;
Hooglugt 0:99cbc87af37c 322 }
Hooglugt 0:99cbc87af37c 323 wait(0.01);
Hooglugt 0:99cbc87af37c 324 }
Hooglugt 4:697d5a806cc4 325 emgcal:
Hooglugt 0:99cbc87af37c 326 blink.detach();
Hooglugt 1:d44a866de64f 327 blink2.detach();
Hooglugt 0:99cbc87af37c 328 dir1 = dir2 = dir3 = 1;
Hooglugt 4:697d5a806cc4 329 for1 = for2 = for3 = 1;
Hooglugt 4:697d5a806cc4 330 pc.printf("kalmoarm ");
Hooglugt 0:99cbc87af37c 331 wait (1);
Hooglugt 0:99cbc87af37c 332 for1 = for2 = for3 = 0;
Hooglugt 0:99cbc87af37c 333
Hooglugt 8:75980dc35763 334 if(kalibratie==0) {
Hooglugt 8:75980dc35763 335 //biceps kalibratie
Hooglugt 8:75980dc35763 336 blink.attach(kalbi, 0.2);
Hooglugt 8:75980dc35763 337 for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
Hooglugt 8:75980dc35763 338 if (bi_max < bi_result) {
Hooglugt 8:75980dc35763 339 bi_max = bi_result;
Hooglugt 8:75980dc35763 340 }
Hooglugt 8:75980dc35763 341 wait (0.01);
Hooglugt 0:99cbc87af37c 342 }
Hooglugt 8:75980dc35763 343 blink.detach();
Hooglugt 8:75980dc35763 344 dir1 = dir2 = dir3 = 1;
Hooglugt 8:75980dc35763 345 pc.printf("kalbi ");
Hooglugt 8:75980dc35763 346 wait (1);
Hooglugt 0:99cbc87af37c 347
Hooglugt 8:75980dc35763 348 //triceps kalibratie
Hooglugt 8:75980dc35763 349 blink.attach(kaltri, 0.2);
Hooglugt 8:75980dc35763 350 for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
Hooglugt 8:75980dc35763 351 if (tri_max < tri_result) {
Hooglugt 8:75980dc35763 352 tri_max = tri_result;
Hooglugt 8:75980dc35763 353 }
Hooglugt 8:75980dc35763 354 wait (0.01);
Hooglugt 0:99cbc87af37c 355 }
Hooglugt 8:75980dc35763 356 blink.detach();
Hooglugt 8:75980dc35763 357 for1 = for2 = for3 = 1;
Hooglugt 8:75980dc35763 358 pc.printf("kaltri ");
Hooglugt 8:75980dc35763 359 wait (1);
Hooglugt 8:75980dc35763 360 for1 = for2 = for3 = 0;
Hooglugt 8:75980dc35763 361 kalibratie = 1;
Hooglugt 0:99cbc87af37c 362 }
Hooglugt 0:99cbc87af37c 363
Hooglugt 4:697d5a806cc4 364 directionchoice:
Hooglugt 1:d44a866de64f 365 log_timer.attach(looper, TSAMP_EMG);
Hooglugt 19:fe284ddaa88a 366 direction = 1;
Hooglugt 9:0bc7f83b761e 367 force = 1;
Hooglugt 9:0bc7f83b761e 368 goto motorcontrol;
Hooglugt 0:99cbc87af37c 369
Hooglugt 4:697d5a806cc4 370 while(1) { //Loop keuze DIRECTION
Hooglugt 0:99cbc87af37c 371 for(int i=1; i<4; i++) {
Hooglugt 0:99cbc87af37c 372 if(i==1) { //red
Hooglugt 0:99cbc87af37c 373 dir1=1;
Hooglugt 0:99cbc87af37c 374 dir2=0;
Hooglugt 0:99cbc87af37c 375 dir3=0;
Hooglugt 0:99cbc87af37c 376 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 377 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 378 direction = 1;
Hooglugt 4:697d5a806cc4 379 pc.printf("links ");
Hooglugt 0:99cbc87af37c 380 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 381 goto forcechoice; // goes to second while(1) for the deciding the force
Hooglugt 0:99cbc87af37c 382 } else {
Hooglugt 0:99cbc87af37c 383 wait(0.01);
Hooglugt 0:99cbc87af37c 384 }
Hooglugt 0:99cbc87af37c 385 }
Hooglugt 0:99cbc87af37c 386 }
Hooglugt 0:99cbc87af37c 387 if(i==2) { //green
Hooglugt 0:99cbc87af37c 388 dir1 =0;
Hooglugt 0:99cbc87af37c 389 dir2 =1;
Hooglugt 0:99cbc87af37c 390 dir3 =0;
Hooglugt 0:99cbc87af37c 391 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 392 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 393 direction = 2;
Hooglugt 4:697d5a806cc4 394 pc.printf("mid ");
Hooglugt 0:99cbc87af37c 395 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 396 goto forcechoice;
Hooglugt 0:99cbc87af37c 397 } else {
Hooglugt 0:99cbc87af37c 398 wait(0.01);
Hooglugt 0:99cbc87af37c 399 }
Hooglugt 0:99cbc87af37c 400 }
Hooglugt 0:99cbc87af37c 401 }
Hooglugt 0:99cbc87af37c 402 if(i==3) { //blue
Hooglugt 0:99cbc87af37c 403 dir1 =0;
Hooglugt 0:99cbc87af37c 404 dir2 =0;
Hooglugt 0:99cbc87af37c 405 dir3 =1;
Hooglugt 0:99cbc87af37c 406 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 407 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 408 direction = 3;
Hooglugt 4:697d5a806cc4 409 pc.printf("rechts ");
Hooglugt 0:99cbc87af37c 410 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 411 goto forcechoice;
Hooglugt 0:99cbc87af37c 412 } else {
Hooglugt 0:99cbc87af37c 413 wait(0.01);
Hooglugt 0:99cbc87af37c 414 }
Hooglugt 0:99cbc87af37c 415 }
Hooglugt 0:99cbc87af37c 416 }
Hooglugt 0:99cbc87af37c 417 }
Hooglugt 0:99cbc87af37c 418 }
Hooglugt 4:697d5a806cc4 419 forcechoice:
Hooglugt 4:697d5a806cc4 420 while(1) { //Loop keuze FORCE
Hooglugt 0:99cbc87af37c 421 for(int j=1; j<4; j++) {
Hooglugt 0:99cbc87af37c 422 if(j==1) { //red
Hooglugt 0:99cbc87af37c 423 for1=1;
Hooglugt 0:99cbc87af37c 424 for2=0;
Hooglugt 0:99cbc87af37c 425 for3=0;
Hooglugt 0:99cbc87af37c 426 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 427 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 428 for1 = for2 = for3 = 0;
Hooglugt 4:697d5a806cc4 429 pc.printf("reset ");
Hooglugt 0:99cbc87af37c 430 goto directionchoice;
Hooglugt 0:99cbc87af37c 431 } else {
Hooglugt 1:d44a866de64f 432 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 433 force = 1;
Hooglugt 4:697d5a806cc4 434 pc.printf("zwak ");
Hooglugt 0:99cbc87af37c 435 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 436 goto choicesmade;
Hooglugt 0:99cbc87af37c 437 } else {
Hooglugt 0:99cbc87af37c 438 wait(0.01);
Hooglugt 0:99cbc87af37c 439 }
Hooglugt 0:99cbc87af37c 440 }
Hooglugt 0:99cbc87af37c 441 }
Hooglugt 0:99cbc87af37c 442 }
Hooglugt 0:99cbc87af37c 443 if(j==2) { //green
Hooglugt 0:99cbc87af37c 444 for1=0;
Hooglugt 0:99cbc87af37c 445 for2=1;
Hooglugt 0:99cbc87af37c 446 for3=0;
Hooglugt 0:99cbc87af37c 447 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 448 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 449 for1 = for2 = for3 = 0;
Hooglugt 4:697d5a806cc4 450 pc.printf("reset ");
Hooglugt 0:99cbc87af37c 451 goto directionchoice;
Hooglugt 0:99cbc87af37c 452 } else {
Hooglugt 1:d44a866de64f 453 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 454 force = 2;
Hooglugt 4:697d5a806cc4 455 pc.printf("normaal ");
Hooglugt 0:99cbc87af37c 456 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 457 goto choicesmade;
Hooglugt 0:99cbc87af37c 458 } else {
Hooglugt 0:99cbc87af37c 459 wait(0.01);
Hooglugt 0:99cbc87af37c 460 }
Hooglugt 0:99cbc87af37c 461 }
Hooglugt 0:99cbc87af37c 462 }
Hooglugt 0:99cbc87af37c 463 }
Hooglugt 0:99cbc87af37c 464 if(j==3) { //blue
Hooglugt 0:99cbc87af37c 465 for1=0;
Hooglugt 0:99cbc87af37c 466 for2=0;
Hooglugt 0:99cbc87af37c 467 for3=1;
Hooglugt 0:99cbc87af37c 468 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 469 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 470 for1 = for2 = for3 = 0;
Hooglugt 4:697d5a806cc4 471 pc.printf("reset ");
Hooglugt 0:99cbc87af37c 472 goto directionchoice;
Hooglugt 0:99cbc87af37c 473 } else {
Hooglugt 1:d44a866de64f 474 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 475 force = 3;
Hooglugt 4:697d5a806cc4 476 pc.printf("sterk ");
Hooglugt 0:99cbc87af37c 477 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 478 goto choicesmade;
Hooglugt 0:99cbc87af37c 479 } else {
Hooglugt 0:99cbc87af37c 480 wait(0.01);
Hooglugt 0:99cbc87af37c 481 }
Hooglugt 0:99cbc87af37c 482 }
Hooglugt 0:99cbc87af37c 483 }
Hooglugt 0:99cbc87af37c 484 }
Hooglugt 0:99cbc87af37c 485 }
Hooglugt 0:99cbc87af37c 486 }
Hooglugt 0:99cbc87af37c 487
Hooglugt 0:99cbc87af37c 488 choicesmade:
Hooglugt 0:99cbc87af37c 489 blink.attach(okay, 0.2);
Hooglugt 4:697d5a806cc4 490 while(1) {
Hooglugt 4:697d5a806cc4 491 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 492 blink.detach();
Hooglugt 4:697d5a806cc4 493 pc.printf("reset ");
Hooglugt 1:d44a866de64f 494 switch (direction) {
Hooglugt 1:d44a866de64f 495 case 1:
Hooglugt 1:d44a866de64f 496 dir1 = 1;
Hooglugt 1:d44a866de64f 497 for1 = 1;
Hooglugt 1:d44a866de64f 498 for2 = for3 = 0;
Hooglugt 1:d44a866de64f 499 break;
Hooglugt 1:d44a866de64f 500 case 2:
Hooglugt 1:d44a866de64f 501 dir2 = 1;
Hooglugt 1:d44a866de64f 502 for1 = 1;
Hooglugt 1:d44a866de64f 503 for2 = for3 = 0;
Hooglugt 1:d44a866de64f 504 break;
Hooglugt 1:d44a866de64f 505 case 3:
Hooglugt 1:d44a866de64f 506 dir3 = 1;
Hooglugt 1:d44a866de64f 507 for1 = 1;
Hooglugt 1:d44a866de64f 508 for2 = for3 = 0;
Hooglugt 1:d44a866de64f 509 break;
Hooglugt 1:d44a866de64f 510 }
Hooglugt 4:697d5a806cc4 511
Hooglugt 4:697d5a806cc4 512 wait(1); // 1 sec wait, anders reset je meteen ook de biceps keuze
Hooglugt 4:697d5a806cc4 513 goto forcechoice;
Hooglugt 0:99cbc87af37c 514 } else {
Hooglugt 1:d44a866de64f 515 if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) {
Hooglugt 4:697d5a806cc4 516 blink.detach();
Hooglugt 4:697d5a806cc4 517 log_timer.detach();
Hooglugt 4:697d5a806cc4 518 goto motorcontrol;
Hooglugt 0:99cbc87af37c 519 } else {
Hooglugt 0:99cbc87af37c 520 wait(0.01); // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje)
Hooglugt 0:99cbc87af37c 521 }
Hooglugt 0:99cbc87af37c 522 }
Hooglugt 0:99cbc87af37c 523 }
Hooglugt 4:697d5a806cc4 524
Hooglugt 4:697d5a806cc4 525 motorcontrol:
Hooglugt 4:697d5a806cc4 526
Hooglugt 0:99cbc87af37c 527 /* Vanaf hier komt de aansturing van de motor */
Hooglugt 0:99cbc87af37c 528
Hooglugt 8:75980dc35763 529 setpoint1=0;
Hooglugt 8:75980dc35763 530 setpoint2=0;
Hooglugt 8:75980dc35763 531 integral1 = integral = 0;
Hooglugt 8:75980dc35763 532 previouserror1 = previouserror = 0;
Hooglugt 11:b517e73a98ab 533
Hooglugt 11:b517e73a98ab 534
Hooglugt 8:75980dc35763 535 while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
Hooglugt 11:b517e73a98ab 536 while(!looptimerflag);
Hooglugt 9:0bc7f83b761e 537 looptimerflag = false; //clear flag
Hooglugt 11:b517e73a98ab 538
Hooglugt 11:b517e73a98ab 539 scope.set(0, motor2.getPosition()*omrekenfactor2);
Hooglugt 11:b517e73a98ab 540 scope.set(1, setpoint2);
Hooglugt 11:b517e73a98ab 541 scope.set(2, motor1.getPosition()*omrekenfactor1);
Hooglugt 11:b517e73a98ab 542 scope.set(3, setpoint1);
Hooglugt 11:b517e73a98ab 543 scope.set(4, state);
Hooglugt 1:d44a866de64f 544
Hooglugt 11:b517e73a98ab 545 switch(state) {
Hooglugt 11:b517e73a98ab 546 case 1: {
Hooglugt 11:b517e73a98ab 547 setpoint1=0;
Hooglugt 19:fe284ddaa88a 548 //setpoint2 += 1*TSAMP;
Hooglugt 11:b517e73a98ab 549 switch (direction) {
Hooglugt 11:b517e73a98ab 550 case 1:
Hooglugt 22:77d7065cc431 551 pwm_motor2.write(0.8);
Hooglugt 22:77d7065cc431 552 motor2dir = 1;
Hooglugt 22:77d7065cc431 553 count++;
Hooglugt 22:77d7065cc431 554 if (count>50) {
Hooglugt 22:77d7065cc431 555 state=2;
Hooglugt 22:77d7065cc431 556 count = 0;
Hooglugt 22:77d7065cc431 557 pwm_motor2.write(0);
Hooglugt 22:77d7065cc431 558 }
Hooglugt 11:b517e73a98ab 559 break;
Hooglugt 11:b517e73a98ab 560 case 2:
Hooglugt 22:77d7065cc431 561 pwm_motor2.write(0.8);
Hooglugt 22:77d7065cc431 562 motor2dir = 1;
Hooglugt 22:77d7065cc431 563 count++;
Hooglugt 22:77d7065cc431 564 if (count>100) {
Hooglugt 22:77d7065cc431 565 state=2;
Hooglugt 22:77d7065cc431 566 count = 0;
Hooglugt 22:77d7065cc431 567 pwm_motor2.write(0);
Hooglugt 22:77d7065cc431 568 }
Hooglugt 11:b517e73a98ab 569 break;
Hooglugt 11:b517e73a98ab 570 case 3:
Hooglugt 18:5467bcc5cbf5 571 angle = 0.08;
Hooglugt 19:fe284ddaa88a 572 break;
Hooglugt 19:fe284ddaa88a 573 }
Hooglugt 22:77d7065cc431 574 /*
Hooglugt 20:5007ddcd03ef 575 setpoint2 = angle;
Hooglugt 20:5007ddcd03ef 576
Hooglugt 20:5007ddcd03ef 577 if(motor2.getPosition()>angle) { // batjeset (controle bereik)
Hooglugt 20:5007ddcd03ef 578 count = 0;
Hooglugt 19:fe284ddaa88a 579 state=2;
Hooglugt 19:fe284ddaa88a 580 }
Hooglugt 21:779de788eb92 581
Hooglugt 22:77d7065cc431 582
Hooglugt 19:fe284ddaa88a 583 if(setpoint2>angle) { // batjeset (controle bereik)
Hooglugt 18:5467bcc5cbf5 584 setpoint2 = angle; //setpoint2 = motor2.getPosition()*omrekenfactor2;
Hooglugt 11:b517e73a98ab 585 count = 0;
Hooglugt 11:b517e73a98ab 586 state=2;
Hooglugt 22:77d7065cc431 587 } */
Hooglugt 11:b517e73a98ab 588 break;
Hooglugt 9:0bc7f83b761e 589 }
Hooglugt 17:7641d7934b91 590 case 2: {
Hooglugt 11:b517e73a98ab 591 count++;
Hooglugt 11:b517e73a98ab 592 if(count>1000) {
Hooglugt 11:b517e73a98ab 593 count = 0;
Hooglugt 11:b517e73a98ab 594 state = 3;
Hooglugt 11:b517e73a98ab 595 }
Hooglugt 11:b517e73a98ab 596 break;
Hooglugt 9:0bc7f83b761e 597 }
Hooglugt 11:b517e73a98ab 598 case 3: {
Hooglugt 11:b517e73a98ab 599 switch (force) {
Hooglugt 11:b517e73a98ab 600 case 1:
Hooglugt 17:7641d7934b91 601 setpoint1 += 2.5*TSAMP; //6.8*TSAMP;
Hooglugt 11:b517e73a98ab 602 break;
Hooglugt 11:b517e73a98ab 603 case 2:
Hooglugt 11:b517e73a98ab 604 setpoint1 += 0.4*TSAMP; //7.4*TSAMP;
Hooglugt 11:b517e73a98ab 605 break;
Hooglugt 11:b517e73a98ab 606 case 3:
Hooglugt 11:b517e73a98ab 607 setpoint1 += 0.4*TSAMP; //8.0*TSAMP;
Hooglugt 11:b517e73a98ab 608 break;
Hooglugt 11:b517e73a98ab 609 }
Hooglugt 11:b517e73a98ab 610 if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) {
Hooglugt 16:a0a39512bd47 611 setpoint1 = 2.36;
Hooglugt 11:b517e73a98ab 612 state = 4;
Hooglugt 11:b517e73a98ab 613 }
Hooglugt 11:b517e73a98ab 614 break;
Hooglugt 8:75980dc35763 615 }
Hooglugt 11:b517e73a98ab 616 case 4: {
Hooglugt 11:b517e73a98ab 617 setpoint2 -= 0.25*TSAMP;
Hooglugt 11:b517e73a98ab 618 if(setpoint2 < 0.001) { //(abs(setpoint2 - motor2.getPosition()*omrekenfactor2) < 0.1) - op 0 draait hij mogelijk in de arm
Hooglugt 11:b517e73a98ab 619 state = 5;
Hooglugt 11:b517e73a98ab 620 }
Hooglugt 11:b517e73a98ab 621 break;
Hooglugt 9:0bc7f83b761e 622 }
Hooglugt 11:b517e73a98ab 623 case 5: {
Hooglugt 11:b517e73a98ab 624 setpoint1 -= 0.5*TSAMP;
Hooglugt 11:b517e73a98ab 625 if(setpoint1 < 0) {
Hooglugt 11:b517e73a98ab 626 state = 6;
Hooglugt 11:b517e73a98ab 627 }
Hooglugt 11:b517e73a98ab 628 break;
Hooglugt 9:0bc7f83b761e 629 }
Hooglugt 11:b517e73a98ab 630 case 6: {
Hooglugt 11:b517e73a98ab 631 setpoint1 = 0;
Hooglugt 11:b517e73a98ab 632 count++;
Hooglugt 11:b517e73a98ab 633 if(count>3000) {
Hooglugt 11:b517e73a98ab 634 count = 0;
Hooglugt 11:b517e73a98ab 635 state = 1;
Hooglugt 18:5467bcc5cbf5 636 goto motor2cal;
Hooglugt 11:b517e73a98ab 637 }
Hooglugt 11:b517e73a98ab 638 break;
Hooglugt 11:b517e73a98ab 639 }
Hooglugt 11:b517e73a98ab 640 }
Hooglugt 8:75980dc35763 641
Hooglugt 11:b517e73a98ab 642 //motor regeling
Hooglugt 18:5467bcc5cbf5 643
Hooglugt 18:5467bcc5cbf5 644 /*
Hooglugt 11:b517e73a98ab 645 //regelaar motor1, bepaalt positie
Hooglugt 11:b517e73a98ab 646 controlerror1 = setpoint1 - motor1.getPosition()*omrekenfactor1;
Hooglugt 11:b517e73a98ab 647 integral1 = integral1 + controlerror1*TSAMP;
Hooglugt 11:b517e73a98ab 648 derivative1 = (controlerror1 - previouserror1)/TSAMP;
Hooglugt 11:b517e73a98ab 649 pwm1 = Kp1*controlerror1 + Ki1*integral1 + Kd1*derivative1;
Hooglugt 11:b517e73a98ab 650 previouserror1 = controlerror1;
Hooglugt 16:a0a39512bd47 651 scope.set(5, pwm1);
Hooglugt 16:a0a39512bd47 652
Hooglugt 11:b517e73a98ab 653 keep_in_range(&pwm1, -1,1);
Hooglugt 11:b517e73a98ab 654 pwm_motor1.write(fabs(pwm1));
Hooglugt 11:b517e73a98ab 655 if(pwm1 > 0) {
Hooglugt 11:b517e73a98ab 656 motor1dir = 1;
Hooglugt 11:b517e73a98ab 657 } else {
Hooglugt 11:b517e73a98ab 658 motor1dir = 0;
Hooglugt 11:b517e73a98ab 659 }
Hooglugt 18:5467bcc5cbf5 660 */
Hooglugt 22:77d7065cc431 661 /*
Hooglugt 11:b517e73a98ab 662 //regelaar motor2, bepaalt positie
Hooglugt 11:b517e73a98ab 663 controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;
Hooglugt 11:b517e73a98ab 664 integral = integral + controlerror*TSAMP;
Hooglugt 11:b517e73a98ab 665 derivative = (controlerror - previouserror)/TSAMP;
Hooglugt 11:b517e73a98ab 666 pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative;
Hooglugt 11:b517e73a98ab 667 previouserror = controlerror;
Hooglugt 11:b517e73a98ab 668
Hooglugt 11:b517e73a98ab 669 keep_in_range(&pwm, -1,1);
Hooglugt 11:b517e73a98ab 670 pwm_motor2.write(fabs(pwm));
Hooglugt 11:b517e73a98ab 671 if(pwm > 0) {
Hooglugt 11:b517e73a98ab 672 motor2dir = 1;
Hooglugt 11:b517e73a98ab 673 } else {
Hooglugt 11:b517e73a98ab 674 motor2dir = 0;
Hooglugt 11:b517e73a98ab 675 }
Hooglugt 22:77d7065cc431 676 */
Hooglugt 11:b517e73a98ab 677 /*
Hooglugt 11:b517e73a98ab 678 //controleert of batje positie heeft bepaald
Hooglugt 11:b517e73a98ab 679 if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
Hooglugt 11:b517e73a98ab 680 if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) {
Hooglugt 11:b517e73a98ab 681 batjeset = 0;
Hooglugt 11:b517e73a98ab 682 } else {
Hooglugt 11:b517e73a98ab 683 batjeset++;
Hooglugt 11:b517e73a98ab 684 }
Hooglugt 11:b517e73a98ab 685 } else {
Hooglugt 11:b517e73a98ab 686 pwm_motor2.write(0);
Hooglugt 11:b517e73a98ab 687 batjeset = integral = derivative = previouserror = 0;
Hooglugt 11:b517e73a98ab 688 wait(1);
Hooglugt 11:b517e73a98ab 689 //goto motor1control;
Hooglugt 11:b517e73a98ab 690 }
Hooglugt 11:b517e73a98ab 691 */
Hooglugt 11:b517e73a98ab 692 }
Hooglugt 11:b517e73a98ab 693 /*
Hooglugt 11:b517e73a98ab 694 motor1control:
Hooglugt 11:b517e73a98ab 695 while(1) { // loop voor het slaan mbv motor1 (batje snelheid)
Hooglugt 11:b517e73a98ab 696 while(!looptimerflag);
Hooglugt 11:b517e73a98ab 697 looptimerflag = false; //clear flag
Hooglugt 11:b517e73a98ab 698
Hooglugt 11:b517e73a98ab 699 if (balhit == 0) { //regelaar motor1, bepaalt snelheid
Hooglugt 11:b517e73a98ab 700 controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
Hooglugt 11:b517e73a98ab 701 integral = integral + controlerror*TSAMP;
Hooglugt 11:b517e73a98ab 702 derivative = (controlerror - previouserror)/TSAMP;
Hooglugt 11:b517e73a98ab 703 pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative;
Hooglugt 11:b517e73a98ab 704 previouserror = controlerror;
Hooglugt 11:b517e73a98ab 705 } else { //regelaar motor1, bepaalt positie
Hooglugt 11:b517e73a98ab 706 balhit = integral = derivative = previouserror = 0;
Hooglugt 11:b517e73a98ab 707 goto resetpositionmotor1;
Hooglugt 11:b517e73a98ab 708 }
Hooglugt 11:b517e73a98ab 709
Hooglugt 11:b517e73a98ab 710 keep_in_range(&pwm, -1,1);
Hooglugt 11:b517e73a98ab 711 pwm_motor1.write(abs(pwm));
Hooglugt 11:b517e73a98ab 712
Hooglugt 11:b517e73a98ab 713 if(pwm > 0) {
Hooglugt 11:b517e73a98ab 714 motor1dir = 1;
Hooglugt 11:b517e73a98ab 715 } else {
Hooglugt 11:b517e73a98ab 716 motor1dir = 0;
Hooglugt 11:b517e73a98ab 717 }
Hooglugt 0:99cbc87af37c 718
Hooglugt 11:b517e73a98ab 719 //controleert of batje balletje heeft bereikt
Hooglugt 11:b517e73a98ab 720 //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
Hooglugt 11:b517e73a98ab 721 if (motor1.getPosition()*omrekenfactor1 > 1.60) {
Hooglugt 11:b517e73a98ab 722 balhit = 1;
Hooglugt 11:b517e73a98ab 723 }
Hooglugt 11:b517e73a98ab 724 }
Hooglugt 11:b517e73a98ab 725 // FORMAT_CODE_END
Hooglugt 11:b517e73a98ab 726
Hooglugt 11:b517e73a98ab 727 resetpositionmotor1:
Hooglugt 11:b517e73a98ab 728 while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst
Hooglugt 11:b517e73a98ab 729 while(!looptimerflag);
Hooglugt 11:b517e73a98ab 730 looptimerflag = false; //clear flag
Hooglugt 11:b517e73a98ab 731
Hooglugt 11:b517e73a98ab 732 //regelaar motor1, bepaalt positie
Hooglugt 11:b517e73a98ab 733 controlerror = -1*motor1.getPosition()*omrekenfactor1;
Hooglugt 11:b517e73a98ab 734 integral = integral + controlerror*TSAMP;
Hooglugt 11:b517e73a98ab 735 derivative = (controlerror - previouserror)/TSAMP;
Hooglugt 11:b517e73a98ab 736 pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative;
Hooglugt 11:b517e73a98ab 737 previouserror = controlerror;
Hooglugt 11:b517e73a98ab 738
Hooglugt 11:b517e73a98ab 739 keep_in_range(&pwm, -1,1);
Hooglugt 11:b517e73a98ab 740 if(pwm > 0) {
Hooglugt 11:b517e73a98ab 741 motor1dir = 1;
Hooglugt 11:b517e73a98ab 742 } else {
Hooglugt 11:b517e73a98ab 743 motor1dir = 0; //1 = rechtsom, 0 = linksom
Hooglugt 11:b517e73a98ab 744 }
Hooglugt 11:b517e73a98ab 745
Hooglugt 11:b517e73a98ab 746 pwm_motor1.write(abs(pwm));
Hooglugt 0:99cbc87af37c 747
Hooglugt 11:b517e73a98ab 748 //controleert of arm terug in positie is
Hooglugt 11:b517e73a98ab 749 if(batjeset < 200) {
Hooglugt 11:b517e73a98ab 750 if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) {
Hooglugt 11:b517e73a98ab 751 batjeset = 0;
Hooglugt 11:b517e73a98ab 752 } else {
Hooglugt 11:b517e73a98ab 753 batjeset++;
Hooglugt 11:b517e73a98ab 754 }
Hooglugt 11:b517e73a98ab 755 } else {
Hooglugt 11:b517e73a98ab 756 pwm_motor1.write(0);
Hooglugt 11:b517e73a98ab 757 batjeset = integral = derivative = previouserror = 0;
Hooglugt 11:b517e73a98ab 758 wait(1);
Hooglugt 11:b517e73a98ab 759 goto resetpositionmotor2;
Hooglugt 11:b517e73a98ab 760 }
Hooglugt 11:b517e73a98ab 761 }
Hooglugt 11:b517e73a98ab 762
Hooglugt 11:b517e73a98ab 763 resetpositionmotor2:
Hooglugt 11:b517e73a98ab 764 while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
Hooglugt 11:b517e73a98ab 765 while(!looptimerflag);
Hooglugt 11:b517e73a98ab 766 looptimerflag = false; //clear flag
Hooglugt 11:b517e73a98ab 767
Hooglugt 11:b517e73a98ab 768 //regelaar motor2, bepaalt positie
Hooglugt 11:b517e73a98ab 769 controlerror = -1*motor2.getPosition()*omrekenfactor2;
Hooglugt 11:b517e73a98ab 770 integral = integral + controlerror*TSAMP;
Hooglugt 11:b517e73a98ab 771 derivative = (controlerror - previouserror)/TSAMP;
Hooglugt 11:b517e73a98ab 772 pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative;
Hooglugt 11:b517e73a98ab 773 previouserror = controlerror;
Hooglugt 11:b517e73a98ab 774
Hooglugt 11:b517e73a98ab 775 keep_in_range(&pwm, -1,1);
Hooglugt 11:b517e73a98ab 776
Hooglugt 11:b517e73a98ab 777 if(pwm > 0) {
Hooglugt 11:b517e73a98ab 778 motor2dir = 1;
Hooglugt 11:b517e73a98ab 779 } else {
Hooglugt 11:b517e73a98ab 780 motor2dir = 0;
Hooglugt 11:b517e73a98ab 781 }
Hooglugt 11:b517e73a98ab 782
Hooglugt 11:b517e73a98ab 783 pwm_motor2.write(abs(pwm));
Hooglugt 11:b517e73a98ab 784
Hooglugt 8:75980dc35763 785 //controleert of batje positie heeft bepaald
Hooglugt 8:75980dc35763 786 if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
Hooglugt 11:b517e73a98ab 787 if (motor2.getPosition()*omrekenfactor2 > 0.1 || motor2.getPosition()*omrekenfactor2 < -0.1) {
Hooglugt 8:75980dc35763 788 batjeset = 0;
Hooglugt 8:75980dc35763 789 } else {
Hooglugt 8:75980dc35763 790 batjeset++;
Hooglugt 8:75980dc35763 791 }
Hooglugt 8:75980dc35763 792 } else {
Hooglugt 8:75980dc35763 793 pwm_motor2.write(0);
Hooglugt 8:75980dc35763 794 batjeset = integral = derivative = previouserror = 0;
Hooglugt 8:75980dc35763 795 wait(1);
Hooglugt 11:b517e73a98ab 796 direction = force = 0;
Hooglugt 11:b517e73a98ab 797 goto motor1cal;
Hooglugt 8:75980dc35763 798 }
Hooglugt 11:b517e73a98ab 799 }*/
Hooglugt 11:b517e73a98ab 800 } // end main