Test to control motors
Dependencies: HIDScope_friso MODSERIAL QEI mbed
Diff: main.cpp
- Revision:
- 0:42272569c45a
- Child:
- 2:8fb37a4587f1
diff -r 000000000000 -r 42272569c45a main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Oct 19 12:48:20 2016 +0000 @@ -0,0 +1,80 @@ +#include "mbed.h" +#include "MODSERIAL.h" +#define SERIAL_BAUD 115200 +#include "QEI.h" +#include "HIDScope.h" + +MODSERIAL pc(USBTX, USBRX); +QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING); +DigitalOut motor1_direction(D4); +PwmOut motor1_pwm(D5); +InterruptIn button1(D2); +AnalogIn potmeter1(A0); + +HIDScope scope(1); +Ticker scopeTimer; + +const float timer = 0.01; +const float timer1 = 0.1; +const float countspr = 8400; +const float max_velocity = 8.4; +const float pi = 3.14159265359; +int counts; +int counts_p; +int pwmvalue; +float potmeter; +float angle; +float referencevelocity; +float velocity; + +void print(){ + pc.printf("%i\r\n", counts); + pc.printf("velocity: %3.3f%\n", velocity); + pc.printf("angle: %3.3f%\n", angle); +} + +void flipdirection() { + motor1_direction = !motor1_direction; +} + +void controlmotor() { + potmeter = potmeter1.read(); + referencevelocity = potmeter*max_velocity; + motor1_pwm.pulsewidth(referencevelocity/max_velocity*0.020); +} + +void calculateangle() { + angle = fmod(counts,countspr)/countspr*2*pi; +} + +void derivative() { + velocity = (counts-counts_p)/countspr*2*pi/timer; + counts_p = counts; +} + +void scopeSend() +{ + scope.set(0,velocity); + scope.send(); +} + +int main() +{ + motor1_direction = 1; + pc.baud(SERIAL_BAUD); + Ticker time; + Ticker time1; + Ticker time2; + Ticker time3; + time.attach(print, timer1); + time1.attach(controlmotor, timer); + time2.attach(calculateangle, timer); + time3.attach(derivative, timer); + button1.rise(&flipdirection); + + scopeTimer.attach_us(&scopeSend, 1e4); + motor1_pwm.period(0.020f); // set pwm period to 20ms + while (true) { + counts = Encoder.getPulses(); + } +} \ No newline at end of file