Matthew Marino / Mbed 2 deprecated encoderread_copy

4/4/19

Dependencies:   mbed QEI

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?

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