![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
final
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?
User | Revision | Line number | New 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 | } |