![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
stuff
main.cpp@0:e6c8377a69d0, 2019-04-04 (annotated)
- Committer:
- LukeMar
- Date:
- Thu Apr 04 18:01:56 2019 +0000
- Revision:
- 0:e6c8377a69d0
4/4
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LukeMar | 0:e6c8377a69d0 | 1 | #include "mbed.h" |
LukeMar | 0:e6c8377a69d0 | 2 | #include <stdio.h> |
LukeMar | 0:e6c8377a69d0 | 3 | #include <stdlib.h> |
LukeMar | 0:e6c8377a69d0 | 4 | #include <QEI.h> |
LukeMar | 0:e6c8377a69d0 | 5 | |
LukeMar | 0:e6c8377a69d0 | 6 | |
LukeMar | 0:e6c8377a69d0 | 7 | DigitalOut led1(LED1); |
LukeMar | 0:e6c8377a69d0 | 8 | PwmOut pwm(p25); |
LukeMar | 0:e6c8377a69d0 | 9 | DigitalOut dir(p27); |
LukeMar | 0:e6c8377a69d0 | 10 | Serial pc(USBTX,USBRX); |
LukeMar | 0:e6c8377a69d0 | 11 | QEI turret(p17, p18, NC, 1600); |
LukeMar | 0:e6c8377a69d0 | 12 | Timer t; |
LukeMar | 0:e6c8377a69d0 | 13 | |
LukeMar | 0:e6c8377a69d0 | 14 | float pulses; |
LukeMar | 0:e6c8377a69d0 | 15 | float revs; |
LukeMar | 0:e6c8377a69d0 | 16 | float dc; |
LukeMar | 0:e6c8377a69d0 | 17 | float theta; |
LukeMar | 0:e6c8377a69d0 | 18 | float err = 0.0; |
LukeMar | 0:e6c8377a69d0 | 19 | float err_old = 0.0; |
LukeMar | 0:e6c8377a69d0 | 20 | float err_ancient = 0.0; |
LukeMar | 0:e6c8377a69d0 | 21 | float dc_old = 0.0; |
LukeMar | 0:e6c8377a69d0 | 22 | float dc_ancient = 0.0; |
LukeMar | 0:e6c8377a69d0 | 23 | float des_theta=0.0; |
LukeMar | 0:e6c8377a69d0 | 24 | |
LukeMar | 0:e6c8377a69d0 | 25 | //float t; |
LukeMar | 0:e6c8377a69d0 | 26 | //float t_2; |
LukeMar | 0:e6c8377a69d0 | 27 | int yes; |
LukeMar | 0:e6c8377a69d0 | 28 | |
LukeMar | 0:e6c8377a69d0 | 29 | Ticker Controller; |
LukeMar | 0:e6c8377a69d0 | 30 | void CtrCode() { |
LukeMar | 0:e6c8377a69d0 | 31 | pulses = turret.getPulses(); |
LukeMar | 0:e6c8377a69d0 | 32 | revs = -pulses / 3200.0; |
LukeMar | 0:e6c8377a69d0 | 33 | theta = revs*360.0; |
LukeMar | 0:e6c8377a69d0 | 34 | |
LukeMar | 0:e6c8377a69d0 | 35 | err = des_theta-theta; |
LukeMar | 0:e6c8377a69d0 | 36 | |
LukeMar | 0:e6c8377a69d0 | 37 | dc = 1.9703*dc_old - 0.9703*dc_ancient + .1471*err- 0.293*err_old + 0.146*err_ancient; //1.9703*dc_old - 0.9703*dc_ancient + .1471*err - 0.293*err_old + 0.146*err_ancient; |
LukeMar | 0:e6c8377a69d0 | 38 | |
LukeMar | 0:e6c8377a69d0 | 39 | err_ancient = err_old; |
LukeMar | 0:e6c8377a69d0 | 40 | err_old = err; |
LukeMar | 0:e6c8377a69d0 | 41 | dc_ancient = dc_old; |
LukeMar | 0:e6c8377a69d0 | 42 | dc_old = dc; |
LukeMar | 0:e6c8377a69d0 | 43 | |
LukeMar | 0:e6c8377a69d0 | 44 | if (dc>0) |
LukeMar | 0:e6c8377a69d0 | 45 | { |
LukeMar | 0:e6c8377a69d0 | 46 | dir = 1; |
LukeMar | 0:e6c8377a69d0 | 47 | } else |
LukeMar | 0:e6c8377a69d0 | 48 | { |
LukeMar | 0:e6c8377a69d0 | 49 | dir = 0; |
LukeMar | 0:e6c8377a69d0 | 50 | } |
LukeMar | 0:e6c8377a69d0 | 51 | |
LukeMar | 0:e6c8377a69d0 | 52 | pwm.write(abs(dc)); |
LukeMar | 0:e6c8377a69d0 | 53 | |
LukeMar | 0:e6c8377a69d0 | 54 | } |
LukeMar | 0:e6c8377a69d0 | 55 | |
LukeMar | 0:e6c8377a69d0 | 56 | |
LukeMar | 0:e6c8377a69d0 | 57 | |
LukeMar | 0:e6c8377a69d0 | 58 | int main() |
LukeMar | 0:e6c8377a69d0 | 59 | { |
LukeMar | 0:e6c8377a69d0 | 60 | float pwm_now; |
LukeMar | 0:e6c8377a69d0 | 61 | |
LukeMar | 0:e6c8377a69d0 | 62 | pc.baud(9600); // set baud rate |
LukeMar | 0:e6c8377a69d0 | 63 | |
LukeMar | 0:e6c8377a69d0 | 64 | dir=1; |
LukeMar | 0:e6c8377a69d0 | 65 | pwm.period(1.0/(20*10^3)); |
LukeMar | 0:e6c8377a69d0 | 66 | pc.printf("start?"); |
LukeMar | 0:e6c8377a69d0 | 67 | pc.scanf("%d",&yes); |
LukeMar | 0:e6c8377a69d0 | 68 | Controller.attach(&CtrCode,.0083); |
LukeMar | 0:e6c8377a69d0 | 69 | des_theta = 90.0; |
LukeMar | 0:e6c8377a69d0 | 70 | |
LukeMar | 0:e6c8377a69d0 | 71 | while(1) { |
LukeMar | 0:e6c8377a69d0 | 72 | pc.printf("Error: %.3f, DC: %.3f\n", err, dc); |
LukeMar | 0:e6c8377a69d0 | 73 | |
LukeMar | 0:e6c8377a69d0 | 74 | |
LukeMar | 0:e6c8377a69d0 | 75 | |
LukeMar | 0:e6c8377a69d0 | 76 | } |
LukeMar | 0:e6c8377a69d0 | 77 | |
LukeMar | 0:e6c8377a69d0 | 78 | |
LukeMar | 0:e6c8377a69d0 | 79 | |
LukeMar | 0:e6c8377a69d0 | 80 | |
LukeMar | 0:e6c8377a69d0 | 81 | } |