Friso van den Boom / Mbed 2 deprecated PIDtest

Test to control motors

Dependencies:   HIDScope_friso MODSERIAL QEI mbed

main.cpp@4:f6ca4f7793bd, 2016-10-25 (annotated)

Committer:
FR150
Date:
Tue Oct 25 12:41:15 2016 +0000
Revision:
4:f6ca4f7793bd
Parent:
3:62e950f20604 
PIDtest werkend

Who changed what in which revision?

UserRevisionLine numberNew contents of line
FR150 0:42272569c45a 1 #include "mbed.h"
FR150 2:8fb37a4587f1 2 #include "MODSERIAL.h" //for serial connection w/ PUTTY
FR150 0:42272569c45a 3 #define SERIAL_BAUD 115200
FR150 2:8fb37a4587f1 4 #include "QEI.h" //for encoder readout
FR150 2:8fb37a4587f1 5 #include "HIDScope.h" //HIDScope
FR150 0:42272569c45a 6
FR150 2:8fb37a4587f1 7 MODSERIAL pc(USBTX, USBRX); //initiate serial connection
FR150 2:8fb37a4587f1 8 QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING); //Initiate encoder readout with pins D12, D13, 64 pulses per rotation and x4 encoding
FR150 2:8fb37a4587f1 9 DigitalOut motor1_direction(D4); //Pin used to control motor direction
FR150 2:8fb37a4587f1 10 PwmOut motor1_pwm(D5); //Pin used to control motor speed
FR150 2:8fb37a4587f1 11 InterruptIn button1(D2); //button
FR150 2:8fb37a4587f1 12 InterruptIn button2(D3); //button
FR150 2:8fb37a4587f1 13 AnalogIn potmeter1(A0); //potmeter
FR150 0:42272569c45a 14
FR150 2:8fb37a4587f1 15 HIDScope scope(3); //initiate HIDScope with 2 channesl
FR150 2:8fb37a4587f1 16 Ticker scopeTimer; //Ticker for HIDScope
FR150 2:8fb37a4587f1 17
FR150 2:8fb37a4587f1 18 Timeout timeout;
FR150 0:42272569c45a 19
FR150 2:8fb37a4587f1 20 const float timer = 0.01; //refresh timer
FR150 2:8fb37a4587f1 21 const float timer1 = 0.1; //refresh timer
FR150 2:8fb37a4587f1 22 const float countspr = 8400; //counts per rotation of motor axis
FR150 2:8fb37a4587f1 23 const float max_velocity = 5.2; //freespin max velocity of motor
FR150 2:8fb37a4587f1 24 const float pi = 3.14159265359; //delicious pi
FR150 2:8fb37a4587f1 25 int counts; //counts from encoder
FR150 2:8fb37a4587f1 26 int counts_p; //previous counts from encoder
FR150 2:8fb37a4587f1 27 int pwmvalue; //pwm value for motor control
FR150 2:8fb37a4587f1 28 float buttoncount;
FR150 2:8fb37a4587f1 29 float potmeter; //potmeter from 0-1
FR150 2:8fb37a4587f1 30 float referenceangle;
FR150 2:8fb37a4587f1 31 float referenceangle_p = 0;
FR150 2:8fb37a4587f1 32 float angle = 0; //angle of motor axis
FR150 2:8fb37a4587f1 33 float angleerror;
FR150 2:8fb37a4587f1 34 float referencevelocity; //controllable velocity
FR150 2:8fb37a4587f1 35 float velocity; //calculated velocity from encoder
FR150 0:42272569c45a 36
FR150 2:8fb37a4587f1 37 void subtractcount();
FR150 2:8fb37a4587f1 38
FR150 2:8fb37a4587f1 39 void print(){ //print over serial connection
FR150 0:42272569c45a 40 pc.printf("%i\r\n", counts);
FR150 2:8fb37a4587f1 41 pc.printf("%1.1f%\r\n", buttoncount);
FR150 2:8fb37a4587f1 42 pc.printf("rangle: %3.3f%\r\n", referenceangle);
FR150 2:8fb37a4587f1 43 pc.printf("angle: %3.3f%\r\n", angle);
FR150 2:8fb37a4587f1 44 pc.printf("error: %3.3f%\r\n", angleerror);
FR150 2:8fb37a4587f1 45 pc.printf("velocity: %3.3f%\r\n", velocity);
FR150 0:42272569c45a 46 }
FR150 0:42272569c45a 47
FR150 2:8fb37a4587f1 48 void controlmotor() { //set motor speed
FR150 2:8fb37a4587f1 49 angleerror = referenceangle-angle;
FR150 3:62e950f20604 50 if (angleerror < 0) motor1_direction = 1;
FR150 3:62e950f20604 51 if (angleerror > 0) motor1_direction = 0;
FR150 0:42272569c45a 52 potmeter = potmeter1.read();
FR150 0:42272569c45a 53 referencevelocity = potmeter*max_velocity;
FR150 4:f6ca4f7793bd 54 motor1_pwm.pulsewidth(fabs(angleerror)*3*referencevelocity/max_velocity*0.020);
FR150 0:42272569c45a 55 }
FR150 0:42272569c45a 56
FR150 2:8fb37a4587f1 57 void calculateangle() { //calculate the angle of the axis based on counts
FR150 2:8fb37a4587f1 58 if (counts == 0) angle = 0;
FR150 3:62e950f20604 59 else angle = -counts/countspr*2*pi;
FR150 0:42272569c45a 60 }
FR150 0:42272569c45a 61
FR150 2:8fb37a4587f1 62 void derivative() { //calculate velocity based on derivative
FR150 0:42272569c45a 63 velocity = (counts-counts_p)/countspr*2*pi/timer;
FR150 0:42272569c45a 64 counts_p = counts;
FR150 0:42272569c45a 65 }
FR150 0:42272569c45a 66
FR150 2:8fb37a4587f1 67 void scopeSend() {//send data over HIDScope
FR150 0:42272569c45a 68 scope.set(0,velocity);
FR150 2:8fb37a4587f1 69 scope.set(1,angle);
FR150 2:8fb37a4587f1 70 scope.set(2,referenceangle);
FR150 0:42272569c45a 71 scope.send();
FR150 0:42272569c45a 72 }
FR150 2:8fb37a4587f1 73
FR150 2:8fb37a4587f1 74 void calculatereferenceangle() {
FR150 2:8fb37a4587f1 75 referenceangle = referenceangle_p+(buttoncount*pi/10);
FR150 2:8fb37a4587f1 76 if(referenceangle < -pi) referenceangle = -pi;
FR150 2:8fb37a4587f1 77 else if(referenceangle > pi) referenceangle = pi;
FR150 2:8fb37a4587f1 78 referenceangle_p = referenceangle;
FR150 2:8fb37a4587f1 79 buttoncount = 0;
FR150 2:8fb37a4587f1 80 }
FR150 2:8fb37a4587f1 81 void addcount() {
FR150 2:8fb37a4587f1 82 buttoncount++;
FR150 2:8fb37a4587f1 83 button1.rise(&addcount);
FR150 2:8fb37a4587f1 84 button2.rise(&subtractcount);
FR150 2:8fb37a4587f1 85 timeout.attach(calculatereferenceangle, 0.5);
FR150 2:8fb37a4587f1 86 }
FR150 2:8fb37a4587f1 87 void subtractcount() {
FR150 2:8fb37a4587f1 88 buttoncount--;
FR150 2:8fb37a4587f1 89 button1.rise(&addcount);
FR150 2:8fb37a4587f1 90 button2.rise(&subtractcount);
FR150 2:8fb37a4587f1 91 timeout.attach(calculatereferenceangle, 0.5);
FR150 2:8fb37a4587f1 92 }
FR150 0:42272569c45a 93 int main()
FR150 0:42272569c45a 94 {
FR150 0:42272569c45a 95 pc.baud(SERIAL_BAUD);
FR150 0:42272569c45a 96 Ticker time;
FR150 0:42272569c45a 97 Ticker time1;
FR150 0:42272569c45a 98 Ticker time2;
FR150 0:42272569c45a 99 Ticker time3;
FR150 0:42272569c45a 100 time.attach(print, timer1);
FR150 0:42272569c45a 101 time1.attach(controlmotor, timer);
FR150 0:42272569c45a 102 time2.attach(calculateangle, timer);
FR150 0:42272569c45a 103 time3.attach(derivative, timer);
FR150 2:8fb37a4587f1 104 button1.rise(&addcount);
FR150 2:8fb37a4587f1 105 button2.rise(&subtractcount);
FR150 0:42272569c45a 106
FR150 0:42272569c45a 107 scopeTimer.attach_us(&scopeSend, 1e4);
FR150 0:42272569c45a 108 motor1_pwm.period(0.020f); // set pwm period to 20ms
FR150 0:42272569c45a 109 while (true) {
FR150 0:42272569c45a 110 counts = Encoder.getPulses();
FR150 0:42272569c45a 111 }
FR150 0:42272569c45a 112 }