![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
4/4/19
main.cpp@0:a859fb8b0f6b, 2019-04-04 (annotated)
- Committer:
- LukeMar
- Date:
- Thu Apr 04 17:53:54 2019 +0000
- Revision:
- 0:a859fb8b0f6b
EW306 control stuff
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LukeMar | 0:a859fb8b0f6b | 1 | #include "mbed.h" |
LukeMar | 0:a859fb8b0f6b | 2 | #include <stdio.h> |
LukeMar | 0:a859fb8b0f6b | 3 | #include <stdlib.h> |
LukeMar | 0:a859fb8b0f6b | 4 | #include <QEI.h> |
LukeMar | 0:a859fb8b0f6b | 5 | |
LukeMar | 0:a859fb8b0f6b | 6 | |
LukeMar | 0:a859fb8b0f6b | 7 | DigitalOut led1(LED1); |
LukeMar | 0:a859fb8b0f6b | 8 | PwmOut pwm(p25); |
LukeMar | 0:a859fb8b0f6b | 9 | DigitalOut dir(p27); |
LukeMar | 0:a859fb8b0f6b | 10 | Serial pc(USBTX,USBRX); |
LukeMar | 0:a859fb8b0f6b | 11 | QEI turret(p17, p18, NC, 1600); |
LukeMar | 0:a859fb8b0f6b | 12 | Timer t; |
LukeMar | 0:a859fb8b0f6b | 13 | |
LukeMar | 0:a859fb8b0f6b | 14 | float pulses; |
LukeMar | 0:a859fb8b0f6b | 15 | float revs; |
LukeMar | 0:a859fb8b0f6b | 16 | float dc; |
LukeMar | 0:a859fb8b0f6b | 17 | float theta; |
LukeMar | 0:a859fb8b0f6b | 18 | float err = 0.0; |
LukeMar | 0:a859fb8b0f6b | 19 | float err_old = 0.0; |
LukeMar | 0:a859fb8b0f6b | 20 | float err_ancient = 0.0; |
LukeMar | 0:a859fb8b0f6b | 21 | float dc_old = 0.0; |
LukeMar | 0:a859fb8b0f6b | 22 | float dc_ancient = 0.0; |
LukeMar | 0:a859fb8b0f6b | 23 | float des_theta=0.0; |
LukeMar | 0:a859fb8b0f6b | 24 | |
LukeMar | 0:a859fb8b0f6b | 25 | //float t; |
LukeMar | 0:a859fb8b0f6b | 26 | //float t_2; |
LukeMar | 0:a859fb8b0f6b | 27 | int yes; |
LukeMar | 0:a859fb8b0f6b | 28 | |
LukeMar | 0:a859fb8b0f6b | 29 | /*ticker controller(){ |
LukeMar | 0:a859fb8b0f6b | 30 | |
LukeMar | 0:a859fb8b0f6b | 31 | |
LukeMar | 0:a859fb8b0f6b | 32 | pc.printf("Pulses is: %i\n\r", turret.getPulses()); |
LukeMar | 0:a859fb8b0f6b | 33 | pc.printf("State is: %i\n\r", turret.getCurrentState()); |
LukeMar | 0:a859fb8b0f6b | 34 | pc.printf("State is: %i\n\r", turret.getRevolutions()); |
LukeMar | 0:a859fb8b0f6b | 35 | }*/ |
LukeMar | 0:a859fb8b0f6b | 36 | |
LukeMar | 0:a859fb8b0f6b | 37 | int main() |
LukeMar | 0:a859fb8b0f6b | 38 | { |
LukeMar | 0:a859fb8b0f6b | 39 | float t_now; |
LukeMar | 0:a859fb8b0f6b | 40 | float pwm_now; |
LukeMar | 0:a859fb8b0f6b | 41 | |
LukeMar | 0:a859fb8b0f6b | 42 | pc.baud(9600); // set baud rate |
LukeMar | 0:a859fb8b0f6b | 43 | |
LukeMar | 0:a859fb8b0f6b | 44 | dir=1; |
LukeMar | 0:a859fb8b0f6b | 45 | pwm.period(1.0/(20*10^3)); |
LukeMar | 0:a859fb8b0f6b | 46 | //pwm.write(abs(dc)); |
LukeMar | 0:a859fb8b0f6b | 47 | //reader.attach(&reading, 0.5); |
LukeMar | 0:a859fb8b0f6b | 48 | while(1) { |
LukeMar | 0:a859fb8b0f6b | 49 | pc.printf("start?"); |
LukeMar | 0:a859fb8b0f6b | 50 | pc.scanf("%d",&yes); |
LukeMar | 0:a859fb8b0f6b | 51 | |
LukeMar | 0:a859fb8b0f6b | 52 | t.start(); |
LukeMar | 0:a859fb8b0f6b | 53 | //pwm_now = 0.5; |
LukeMar | 0:a859fb8b0f6b | 54 | //pwm.write(pwm_now); |
LukeMar | 0:a859fb8b0f6b | 55 | while( t.read() <= 20 ) { |
LukeMar | 0:a859fb8b0f6b | 56 | /*if (t.read() <= 5 ){ |
LukeMar | 0:a859fb8b0f6b | 57 | des_angle = 45; |
LukeMar | 0:a859fb8b0f6b | 58 | } |
LukeMar | 0:a859fb8b0f6b | 59 | if (t.read() <= 10 && t.read() >5 ){ |
LukeMar | 0:a859fb8b0f6b | 60 | des_angle = 90; |
LukeMar | 0:a859fb8b0f6b | 61 | } |
LukeMar | 0:a859fb8b0f6b | 62 | if (t.read() <= 15 && t.read()>10){ |
LukeMar | 0:a859fb8b0f6b | 63 | des_angle = 125; |
LukeMar | 0:a859fb8b0f6b | 64 | } |
LukeMar | 0:a859fb8b0f6b | 65 | if (t.read()<= 20 && t.read() >15){ |
LukeMar | 0:a859fb8b0f6b | 66 | des_angle = 180; |
LukeMar | 0:a859fb8b0f6b | 67 | }*/ |
LukeMar | 0:a859fb8b0f6b | 68 | des_theta = 45.0; |
LukeMar | 0:a859fb8b0f6b | 69 | pulses = turret.getPulses(); |
LukeMar | 0:a859fb8b0f6b | 70 | revs = -pulses / 3200.0; |
LukeMar | 0:a859fb8b0f6b | 71 | theta = revs*360.0; |
LukeMar | 0:a859fb8b0f6b | 72 | |
LukeMar | 0:a859fb8b0f6b | 73 | err = des_theta-theta; |
LukeMar | 0:a859fb8b0f6b | 74 | |
LukeMar | 0:a859fb8b0f6b | 75 | dc = 1.9703*dc_old - 0.9703*dc_ancient + 0.01471*err - 0.0293*err_old + 0.0146*err_ancient; |
LukeMar | 0:a859fb8b0f6b | 76 | |
LukeMar | 0:a859fb8b0f6b | 77 | err_ancient = err_old; |
LukeMar | 0:a859fb8b0f6b | 78 | err_old = err; |
LukeMar | 0:a859fb8b0f6b | 79 | dc_ancient = dc_old; |
LukeMar | 0:a859fb8b0f6b | 80 | dc_old = dc; |
LukeMar | 0:a859fb8b0f6b | 81 | |
LukeMar | 0:a859fb8b0f6b | 82 | |
LukeMar | 0:a859fb8b0f6b | 83 | t_now = t.read(); |
LukeMar | 0:a859fb8b0f6b | 84 | |
LukeMar | 0:a859fb8b0f6b | 85 | if (dc>0) { |
LukeMar | 0:a859fb8b0f6b | 86 | dir = 1; |
LukeMar | 0:a859fb8b0f6b | 87 | } else { |
LukeMar | 0:a859fb8b0f6b | 88 | dir = 0; |
LukeMar | 0:a859fb8b0f6b | 89 | } |
LukeMar | 0:a859fb8b0f6b | 90 | pc.printf("Error: %.3f, DC: %.3f\n", err, dc); |
LukeMar | 0:a859fb8b0f6b | 91 | pwm.write(abs(dc)); |
LukeMar | 0:a859fb8b0f6b | 92 | |
LukeMar | 0:a859fb8b0f6b | 93 | } |
LukeMar | 0:a859fb8b0f6b | 94 | |
LukeMar | 0:a859fb8b0f6b | 95 | |
LukeMar | 0:a859fb8b0f6b | 96 | |
LukeMar | 0:a859fb8b0f6b | 97 | |
LukeMar | 0:a859fb8b0f6b | 98 | |
LukeMar | 0:a859fb8b0f6b | 99 | wait(5); |
LukeMar | 0:a859fb8b0f6b | 100 | } |
LukeMar | 0:a859fb8b0f6b | 101 | |
LukeMar | 0:a859fb8b0f6b | 102 | |
LukeMar | 0:a859fb8b0f6b | 103 | } |