![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
stuff
main.cpp
- Committer:
- LukeMar
- Date:
- 2019-04-04
- Revision:
- 0:e6c8377a69d0
File content as of revision 0:e6c8377a69d0:
#include "mbed.h" #include <stdio.h> #include <stdlib.h> #include <QEI.h> DigitalOut led1(LED1); PwmOut pwm(p25); DigitalOut dir(p27); Serial pc(USBTX,USBRX); QEI turret(p17, p18, NC, 1600); Timer t; float pulses; float revs; float dc; float theta; float err = 0.0; float err_old = 0.0; float err_ancient = 0.0; float dc_old = 0.0; float dc_ancient = 0.0; float des_theta=0.0; //float t; //float t_2; int yes; Ticker Controller; void CtrCode() { pulses = turret.getPulses(); revs = -pulses / 3200.0; theta = revs*360.0; err = des_theta-theta; 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; err_ancient = err_old; err_old = err; dc_ancient = dc_old; dc_old = dc; if (dc>0) { dir = 1; } else { dir = 0; } pwm.write(abs(dc)); } int main() { float pwm_now; pc.baud(9600); // set baud rate dir=1; pwm.period(1.0/(20*10^3)); pc.printf("start?"); pc.scanf("%d",&yes); Controller.attach(&CtrCode,.0083); des_theta = 90.0; while(1) { pc.printf("Error: %.3f, DC: %.3f\n", err, dc); } }