Matthew Marino / Mbed 2 deprecated EW309_Controller

stuff

Dependencies:   mbed QEI

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?

UserRevisionLine numberNew 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 }