Aukie Hooglugt / Mbed 2 deprecated BMT-M9_motorcontrol

2 sec interval: rechtsom, linksom, pauze

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

Fork of BMT-M9_motorcontrol_groep3 by Aukie Hooglugt

main.cpp@17:055edbd6ba9d, 2014-10-20 (annotated)

Committer:
Hooglugt
Date:
Mon Oct 20 08:13:51 2014 +0000
Revision:
17:055edbd6ba9d
Parent:
16:1869377fbb73 
step function send to motor 1 and motor 2, (rotates right, left, then pauses)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c9e647421e54 1 #include "mbed.h"
vsluiter 0:c9e647421e54 2 #include "encoder.h"
vsluiter 6:0832c6c6bcba 3 #include "HIDScope.h"
vsluiter 6:0832c6c6bcba 4
vsluiter 5:06381e54b94a 5 #define TSAMP 0.01
vsluiter 6:0832c6c6bcba 6
vsluiter 8:15c6cb82c725 7 void clamp(float * in, float min, float max);
vsluiter 0:c9e647421e54 8 volatile bool looptimerflag;
Hooglugt 17:055edbd6ba9d 9 HIDScope scope(3);
Hooglugt 17:055edbd6ba9d 10 float new_pwm = 0;
vsluiter 6:0832c6c6bcba 11
vsluiter 0:c9e647421e54 12 void setlooptimerflag(void)
vsluiter 0:c9e647421e54 13 {
vsluiter 0:c9e647421e54 14 looptimerflag = true;
vsluiter 0:c9e647421e54 15 }
vsluiter 0:c9e647421e54 16
Hooglugt 13:7ec4762ec4ce 17 void clamp(float * in, float min, float max)
Hooglugt 13:7ec4762ec4ce 18 {
Hooglugt 13:7ec4762ec4ce 19 *in > min ? *in < max? : *in = max: *in = min;
Hooglugt 13:7ec4762ec4ce 20 }
vsluiter 1:5ac85aad9da4 21
Hooglugt 14:98925c1ed440 22 void looper()
Hooglugt 14:98925c1ed440 23 {
Hooglugt 17:055edbd6ba9d 24 if (new_pwm==1) {
Hooglugt 17:055edbd6ba9d 25 new_pwm = -1;
Hooglugt 14:98925c1ed440 26 } else {
Hooglugt 17:055edbd6ba9d 27 if (new_pwm==-1) {
Hooglugt 17:055edbd6ba9d 28 new_pwm = 0;
Hooglugt 17:055edbd6ba9d 29 } else {
Hooglugt 17:055edbd6ba9d 30 if (new_pwm==0) {
Hooglugt 17:055edbd6ba9d 31 new_pwm = 1;
Hooglugt 17:055edbd6ba9d 32 }
Hooglugt 17:055edbd6ba9d 33 }
Hooglugt 14:98925c1ed440 34 }
Hooglugt 14:98925c1ed440 35 }
Hooglugt 14:98925c1ed440 36
vsluiter 4:1a53b06eeb7f 37 int main()
vsluiter 4:1a53b06eeb7f 38 {
Hooglugt 16:1869377fbb73 39 //motor 1, 25D
Hooglugt 15:979011e26ca3 40 Encoder motor1(PTD3, PTD5);
Hooglugt 15:979011e26ca3 41 DigitalOut motor1dir(PTC9);
Hooglugt 15:979011e26ca3 42 PwmOut pwm_motor1(PTC8);
Hooglugt 17:055edbd6ba9d 43 pwm_motor1.period_us(100); //10kHz PWM frequency
Hooglugt 16:1869377fbb73 44
Hooglugt 16:1869377fbb73 45 //motor 2, 25D
Hooglugt 15:979011e26ca3 46 Encoder motor2(PTD2,PTD0);
Hooglugt 15:979011e26ca3 47 DigitalOut motor2dir(PTA4);
Hooglugt 15:979011e26ca3 48 PwmOut pwm_motor2(PTA5);
Hooglugt 17:055edbd6ba9d 49 pwm_motor2.period_us(100); //10kHz PWM frequency
Hooglugt 16:1869377fbb73 50
vsluiter 0:c9e647421e54 51 Ticker looptimer;
vsluiter 5:06381e54b94a 52 looptimer.attach(setlooptimerflag,TSAMP);
Hooglugt 14:98925c1ed440 53 Ticker flip_switch;
Hooglugt 15:979011e26ca3 54 flip_switch.attach(looper, 2);
Hooglugt 13:7ec4762ec4ce 55
vsluiter 0:c9e647421e54 56 while(1) {
vsluiter 0:c9e647421e54 57 while(!looptimerflag);
vsluiter 8:15c6cb82c725 58 looptimerflag = false; //clear flag
Hooglugt 13:7ec4762ec4ce 59
vsluiter 8:15c6cb82c725 60 clamp(&new_pwm, -1,1);
Hooglugt 13:7ec4762ec4ce 61
Hooglugt 13:7ec4762ec4ce 62 scope.set(0, new_pwm);
Hooglugt 17:055edbd6ba9d 63 scope.set(1, motor1.getPosition());
Hooglugt 17:055edbd6ba9d 64 scope.set(2, motor2.getPosition());
vsluiter 6:0832c6c6bcba 65 scope.send();
Hooglugt 14:98925c1ed440 66
vsluiter 0:c9e647421e54 67 if(new_pwm > 0)
Hooglugt 17:055edbd6ba9d 68 motor1dir = 0;
Hooglugt 17:055edbd6ba9d 69 else
Hooglugt 17:055edbd6ba9d 70 motor1dir = 1;
Hooglugt 17:055edbd6ba9d 71
Hooglugt 17:055edbd6ba9d 72 if(new_pwm > 0)
Hooglugt 15:979011e26ca3 73 motor2dir = 0;
vsluiter 7:37b06688b449 74 else
Hooglugt 15:979011e26ca3 75 motor2dir = 1;
Hooglugt 14:98925c1ed440 76
Hooglugt 17:055edbd6ba9d 77 pwm_motor1.write(abs(new_pwm));
Hooglugt 15:979011e26ca3 78 pwm_motor2.write(abs(new_pwm));
vsluiter 0:c9e647421e54 79 }
Hooglugt 13:7ec4762ec4ce 80 }