Friso van den Boom / Mbed 2 deprecated PIDtest

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?

UserRevisionLine numberNew 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 }