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?
User | Revision | Line number | New 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 | } |