ES309 Project Team / Mbed 2 deprecated turret2cpp

final

Dependencies:   Motor QEI mbed

turret.cpp@0:72ae4ee22e56, 2016-05-09 (annotated)

Committer:
caleblegis
Date:
Mon May 09 23:51:59 2016 +0000
Revision:
0:72ae4ee22e56
ss

Who changed what in which revision?

UserRevisionLine numberNew contents of line
caleblegis 0:72ae4ee22e56 1 #include "mbed.h"
caleblegis 0:72ae4ee22e56 2 #include "Motor.h"
caleblegis 0:72ae4ee22e56 3 #include "QEI.h"
caleblegis 0:72ae4ee22e56 4 #define pi (3.14159265)
caleblegis 0:72ae4ee22e56 5 #define logfreq (100)
caleblegis 0:72ae4ee22e56 6 #define data_pts (500)
caleblegis 0:72ae4ee22e56 7 Serial pc(USBTX, USBRX);
caleblegis 0:72ae4ee22e56 8 Motor m(p25, p23, p27);
caleblegis 0:72ae4ee22e56 9
caleblegis 0:72ae4ee22e56 10 QEI enc(p16,p17,NC,1600);
caleblegis 0:72ae4ee22e56 11
caleblegis 0:72ae4ee22e56 12
caleblegis 0:72ae4ee22e56 13
caleblegis 0:72ae4ee22e56 14 float motspeed;
caleblegis 0:72ae4ee22e56 15 float x;
caleblegis 0:72ae4ee22e56 16 float rad;
caleblegis 0:72ae4ee22e56 17 int count;
caleblegis 0:72ae4ee22e56 18 Timer t;
caleblegis 0:72ae4ee22e56 19 float rad_one=0;
caleblegis 0:72ae4ee22e56 20 float time_one=0;
caleblegis 0:72ae4ee22e56 21 float omega;
caleblegis 0:72ae4ee22e56 22 float old_E;
caleblegis 0:72ae4ee22e56 23 float E;
caleblegis 0:72ae4ee22e56 24 float PI=3.14159265;
caleblegis 0:72ae4ee22e56 25 float old_t;
caleblegis 0:72ae4ee22e56 26 float DC;
caleblegis 0:72ae4ee22e56 27 float old_DC;
caleblegis 0:72ae4ee22e56 28 float theta;
caleblegis 0:72ae4ee22e56 29 //float theta_desired=x;
caleblegis 0:72ae4ee22e56 30 float Ts=0.01;
caleblegis 0:72ae4ee22e56 31
caleblegis 0:72ae4ee22e56 32 int main()
caleblegis 0:72ae4ee22e56 33 {
caleblegis 0:72ae4ee22e56 34 t.reset();
caleblegis 0:72ae4ee22e56 35 enc.reset();
caleblegis 0:72ae4ee22e56 36 pc.printf("enter an angle\r\n");
caleblegis 0:72ae4ee22e56 37 pc.scanf("%f", &x);
caleblegis 0:72ae4ee22e56 38
caleblegis 0:72ae4ee22e56 39
caleblegis 0:72ae4ee22e56 40
caleblegis 0:72ae4ee22e56 41
caleblegis 0:72ae4ee22e56 42 while(1) {
caleblegis 0:72ae4ee22e56 43 t.start();
caleblegis 0:72ae4ee22e56 44 wait(1/logfreq);
caleblegis 0:72ae4ee22e56 45 //if(t.read()-old_t>=Ts)
caleblegis 0:72ae4ee22e56 46
caleblegis 0:72ae4ee22e56 47 count = enc.getPulses() ;
caleblegis 0:72ae4ee22e56 48 theta = -count*((2*pi)/3200) ;
caleblegis 0:72ae4ee22e56 49 // Calculate position error
caleblegis 0:72ae4ee22e56 50 E=(x*(PI/180))-theta;
caleblegis 0:72ae4ee22e56 51
caleblegis 0:72ae4ee22e56 52 // Control algorithm
caleblegis 0:72ae4ee22e56 53 DC = ((0.9841*old_DC) + (0.2348*E) - (0.2335*old_E));
caleblegis 0:72ae4ee22e56 54 old_E = E;
caleblegis 0:72ae4ee22e56 55 m.speed(DC+0.15);
caleblegis 0:72ae4ee22e56 56 old_DC = DC;
caleblegis 0:72ae4ee22e56 57
caleblegis 0:72ae4ee22e56 58 if(DC>0 && DC<0.7)
caleblegis 0:72ae4ee22e56 59 m.speed(DC+0.1);
caleblegis 0:72ae4ee22e56 60 if(E>-1.0)
caleblegis 0:72ae4ee22e56 61 m.speed(DC+0.15);
caleblegis 0:72ae4ee22e56 62 if (E<0.015 && E>-0.01)
caleblegis 0:72ae4ee22e56 63 m.speed(0.0);
caleblegis 0:72ae4ee22e56 64 else if(DC<0 && DC>-0.7){
caleblegis 0:72ae4ee22e56 65 m.speed(DC-0.34);
caleblegis 0:72ae4ee22e56 66 if(E>-1.0)
caleblegis 0:72ae4ee22e56 67 m.speed(DC-0.5);
caleblegis 0:72ae4ee22e56 68 if (E>-0.015&& E<0.01)
caleblegis 0:72ae4ee22e56 69 m.speed(0.0);}
caleblegis 0:72ae4ee22e56 70
caleblegis 0:72ae4ee22e56 71 // Age variables
caleblegis 0:72ae4ee22e56 72
caleblegis 0:72ae4ee22e56 73
caleblegis 0:72ae4ee22e56 74 old_t = t.read();
caleblegis 0:72ae4ee22e56 75
caleblegis 0:72ae4ee22e56 76
caleblegis 0:72ae4ee22e56 77 //omega=(rad-rad_one)/(t-time_one);
caleblegis 0:72ae4ee22e56 78 //right is negative, left is positive theta
caleblegis 0:72ae4ee22e56 79
caleblegis 0:72ae4ee22e56 80 //if (x<0){
caleblegis 0:72ae4ee22e56 81 // m.speed(0.25);
caleblegis 0:72ae4ee22e56 82 //if(rad==x){
caleblegis 0:72ae4ee22e56 83 //m.speed(0);}}
caleblegis 0:72ae4ee22e56 84 //if (x>0){
caleblegis 0:72ae4ee22e56 85 // m.speed(-0.5);
caleblegis 0:72ae4ee22e56 86 // if(rad==x){
caleblegis 0:72ae4ee22e56 87 //m.speed(0);}}
caleblegis 0:72ae4ee22e56 88 //rad_one=rad;
caleblegis 0:72ae4ee22e56 89 //time_one=t;
caleblegis 0:72ae4ee22e56 90 printf("time: %f angle: %f error: %f DC: %f\n\r", t.read(), theta, E,DC);
caleblegis 0:72ae4ee22e56 91 }
caleblegis 0:72ae4ee22e56 92
caleblegis 0:72ae4ee22e56 93
caleblegis 0:72ae4ee22e56 94 //0.2 for forward torque
caleblegis 0:72ae4ee22e56 95 //-0.5 for backward
caleblegis 0:72ae4ee22e56 96
caleblegis 0:72ae4ee22e56 97
caleblegis 0:72ae4ee22e56 98 //for (servopos=0; servopos<=1.0; servopos+=0.1){
caleblegis 0:72ae4ee22e56 99 //for (motspeed= 0.2; motspeed <= 1.0; motspeed += 0.1) {
caleblegis 0:72ae4ee22e56 100 //myservo1 = servopos;
caleblegis 0:72ae4ee22e56 101 //myservo2 = 1.0 - servopos;
caleblegis 0:72ae4ee22e56 102 //printf("Servo 1 position %.2f Servo 2 position %.3f\n", servopos, (1-servopos));
caleblegis 0:72ae4ee22e56 103
caleblegis 0:72ae4ee22e56 104 //m.speed(motspeed);
caleblegis 0:72ae4ee22e56 105 //printf("motor speed is %.2f\n", motspeed);
caleblegis 0:72ae4ee22e56 106 //wait(0.0002);
caleblegis 0:72ae4ee22e56 107 //m.speed(-1.0);
caleblegis 0:72ae4ee22e56 108 //wait(0.002);
caleblegis 0:72ae4ee22e56 109 //m.speed(1.0);
caleblegis 0:72ae4ee22e56 110 }