Test to control motors
Dependencies: HIDScope_friso MODSERIAL QEI mbed
main.cpp@0:42272569c45a, 2016-10-19 (annotated)
- Committer:
- FR150
- Date:
- Wed Oct 19 12:48:20 2016 +0000
- Revision:
- 0:42272569c45a
- Child:
- 2:8fb37a4587f1
Motortest with potmeter and velocity calculation, HIDScope not working
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
FR150 | 0:42272569c45a | 1 | #include "mbed.h" |
FR150 | 0:42272569c45a | 2 | #include "MODSERIAL.h" |
FR150 | 0:42272569c45a | 3 | #define SERIAL_BAUD 115200 |
FR150 | 0:42272569c45a | 4 | #include "QEI.h" |
FR150 | 0:42272569c45a | 5 | #include "HIDScope.h" |
FR150 | 0:42272569c45a | 6 | |
FR150 | 0:42272569c45a | 7 | MODSERIAL pc(USBTX, USBRX); |
FR150 | 0:42272569c45a | 8 | QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING); |
FR150 | 0:42272569c45a | 9 | DigitalOut motor1_direction(D4); |
FR150 | 0:42272569c45a | 10 | PwmOut motor1_pwm(D5); |
FR150 | 0:42272569c45a | 11 | InterruptIn button1(D2); |
FR150 | 0:42272569c45a | 12 | AnalogIn potmeter1(A0); |
FR150 | 0:42272569c45a | 13 | |
FR150 | 0:42272569c45a | 14 | HIDScope scope(1); |
FR150 | 0:42272569c45a | 15 | Ticker scopeTimer; |
FR150 | 0:42272569c45a | 16 | |
FR150 | 0:42272569c45a | 17 | const float timer = 0.01; |
FR150 | 0:42272569c45a | 18 | const float timer1 = 0.1; |
FR150 | 0:42272569c45a | 19 | const float countspr = 8400; |
FR150 | 0:42272569c45a | 20 | const float max_velocity = 8.4; |
FR150 | 0:42272569c45a | 21 | const float pi = 3.14159265359; |
FR150 | 0:42272569c45a | 22 | int counts; |
FR150 | 0:42272569c45a | 23 | int counts_p; |
FR150 | 0:42272569c45a | 24 | int pwmvalue; |
FR150 | 0:42272569c45a | 25 | float potmeter; |
FR150 | 0:42272569c45a | 26 | float angle; |
FR150 | 0:42272569c45a | 27 | float referencevelocity; |
FR150 | 0:42272569c45a | 28 | float velocity; |
FR150 | 0:42272569c45a | 29 | |
FR150 | 0:42272569c45a | 30 | void print(){ |
FR150 | 0:42272569c45a | 31 | pc.printf("%i\r\n", counts); |
FR150 | 0:42272569c45a | 32 | pc.printf("velocity: %3.3f%\n", velocity); |
FR150 | 0:42272569c45a | 33 | pc.printf("angle: %3.3f%\n", angle); |
FR150 | 0:42272569c45a | 34 | } |
FR150 | 0:42272569c45a | 35 | |
FR150 | 0:42272569c45a | 36 | void flipdirection() { |
FR150 | 0:42272569c45a | 37 | motor1_direction = !motor1_direction; |
FR150 | 0:42272569c45a | 38 | } |
FR150 | 0:42272569c45a | 39 | |
FR150 | 0:42272569c45a | 40 | void controlmotor() { |
FR150 | 0:42272569c45a | 41 | potmeter = potmeter1.read(); |
FR150 | 0:42272569c45a | 42 | referencevelocity = potmeter*max_velocity; |
FR150 | 0:42272569c45a | 43 | motor1_pwm.pulsewidth(referencevelocity/max_velocity*0.020); |
FR150 | 0:42272569c45a | 44 | } |
FR150 | 0:42272569c45a | 45 | |
FR150 | 0:42272569c45a | 46 | void calculateangle() { |
FR150 | 0:42272569c45a | 47 | angle = fmod(counts,countspr)/countspr*2*pi; |
FR150 | 0:42272569c45a | 48 | } |
FR150 | 0:42272569c45a | 49 | |
FR150 | 0:42272569c45a | 50 | void derivative() { |
FR150 | 0:42272569c45a | 51 | velocity = (counts-counts_p)/countspr*2*pi/timer; |
FR150 | 0:42272569c45a | 52 | counts_p = counts; |
FR150 | 0:42272569c45a | 53 | } |
FR150 | 0:42272569c45a | 54 | |
FR150 | 0:42272569c45a | 55 | void scopeSend() |
FR150 | 0:42272569c45a | 56 | { |
FR150 | 0:42272569c45a | 57 | scope.set(0,velocity); |
FR150 | 0:42272569c45a | 58 | scope.send(); |
FR150 | 0:42272569c45a | 59 | } |
FR150 | 0:42272569c45a | 60 | |
FR150 | 0:42272569c45a | 61 | int main() |
FR150 | 0:42272569c45a | 62 | { |
FR150 | 0:42272569c45a | 63 | motor1_direction = 1; |
FR150 | 0:42272569c45a | 64 | pc.baud(SERIAL_BAUD); |
FR150 | 0:42272569c45a | 65 | Ticker time; |
FR150 | 0:42272569c45a | 66 | Ticker time1; |
FR150 | 0:42272569c45a | 67 | Ticker time2; |
FR150 | 0:42272569c45a | 68 | Ticker time3; |
FR150 | 0:42272569c45a | 69 | time.attach(print, timer1); |
FR150 | 0:42272569c45a | 70 | time1.attach(controlmotor, timer); |
FR150 | 0:42272569c45a | 71 | time2.attach(calculateangle, timer); |
FR150 | 0:42272569c45a | 72 | time3.attach(derivative, timer); |
FR150 | 0:42272569c45a | 73 | button1.rise(&flipdirection); |
FR150 | 0:42272569c45a | 74 | |
FR150 | 0:42272569c45a | 75 | scopeTimer.attach_us(&scopeSend, 1e4); |
FR150 | 0:42272569c45a | 76 | motor1_pwm.period(0.020f); // set pwm period to 20ms |
FR150 | 0:42272569c45a | 77 | while (true) { |
FR150 | 0:42272569c45a | 78 | counts = Encoder.getPulses(); |
FR150 | 0:42272569c45a | 79 | } |
FR150 | 0:42272569c45a | 80 | } |