454
Dependencies: Motor QEI mbed mbedWSEsbc
Fork of ES309_NerfTurretfinal by
main.cpp@3:4dcf81584894, 2016-05-12 (annotated)
- Committer:
- caleblegis
- Date:
- Thu May 12 13:16:33 2016 +0000
- Revision:
- 3:4dcf81584894
- Parent:
- 2:b8a033375c27
- Child:
- 4:806d9d971c26
h
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rixonfletcher | 1:758c16346f28 | 1 | #include "mbed.h" |
rixonfletcher | 2:b8a033375c27 | 2 | #include "Motor.h" |
rixonfletcher | 2:b8a033375c27 | 3 | #include "QEI.h" |
rixonfletcher | 2:b8a033375c27 | 4 | #define pi (3.14159265) |
rixonfletcher | 2:b8a033375c27 | 5 | #define logfreq (100) |
rixonfletcher | 2:b8a033375c27 | 6 | #define data_pts (500) |
rixonfletcher | 2:b8a033375c27 | 7 | |
rixonfletcher | 2:b8a033375c27 | 8 | Serial pc(USBTX, USBRX); |
rixonfletcher | 2:b8a033375c27 | 9 | Motor m(p25, p23, p27); |
rixonfletcher | 2:b8a033375c27 | 10 | QEI enc(p16,p17,NC,1600); |
rixonfletcher | 1:758c16346f28 | 11 | |
rixonfletcher | 1:758c16346f28 | 12 | DigitalOut light[]= {LED1, LED2, LED3, LED4}; |
rixonfletcher | 2:b8a033375c27 | 13 | DigitalOut spinner(p21); |
rixonfletcher | 2:b8a033375c27 | 14 | DigitalOut plunger(p22); |
rixonfletcher | 2:b8a033375c27 | 15 | |
rixonfletcher | 2:b8a033375c27 | 16 | // firing circuit configuration |
rixonfletcher | 2:b8a033375c27 | 17 | int disc=20; //number of discs in magazine |
rixonfletcher | 2:b8a033375c27 | 18 | float shot_time=0.6; //time per shot. 3.51s for 5 shots |
rixonfletcher | 2:b8a033375c27 | 19 | int fire=4; //number of bullets to fire (reset to zero after every round of shots) |
rixonfletcher | 2:b8a033375c27 | 20 | float fwait; //fire time wait |
rixonfletcher | 2:b8a033375c27 | 21 | int shoot; |
rixonfletcher | 2:b8a033375c27 | 22 | |
rixonfletcher | 2:b8a033375c27 | 23 | |
rixonfletcher | 2:b8a033375c27 | 24 | //turret circuit configuration |
rixonfletcher | 2:b8a033375c27 | 25 | float motspeed; |
rixonfletcher | 2:b8a033375c27 | 26 | float x; |
rixonfletcher | 2:b8a033375c27 | 27 | float rad; |
rixonfletcher | 2:b8a033375c27 | 28 | int count; |
rixonfletcher | 2:b8a033375c27 | 29 | Timer t; |
rixonfletcher | 2:b8a033375c27 | 30 | float rad_one=0; |
rixonfletcher | 2:b8a033375c27 | 31 | float time_one=0; |
rixonfletcher | 2:b8a033375c27 | 32 | float omega; //angular velocity |
rixonfletcher | 2:b8a033375c27 | 33 | float old_E; |
rixonfletcher | 2:b8a033375c27 | 34 | float E; // error |
rixonfletcher | 2:b8a033375c27 | 35 | float PI=3.14159265; |
rixonfletcher | 2:b8a033375c27 | 36 | float old_t; |
rixonfletcher | 2:b8a033375c27 | 37 | float DC; |
rixonfletcher | 2:b8a033375c27 | 38 | float old_DC; |
rixonfletcher | 2:b8a033375c27 | 39 | float theta; |
rixonfletcher | 2:b8a033375c27 | 40 | //float theta_desired=x; |
rixonfletcher | 2:b8a033375c27 | 41 | float Ts=0.01; |
rixonfletcher | 2:b8a033375c27 | 42 | |
rixonfletcher | 1:758c16346f28 | 43 | |
rixonfletcher | 1:758c16346f28 | 44 | int main() |
rixonfletcher | 1:758c16346f28 | 45 | { |
rixonfletcher | 2:b8a033375c27 | 46 | // initialize firing settings |
rixonfletcher | 2:b8a033375c27 | 47 | light[0].write(0); |
rixonfletcher | 2:b8a033375c27 | 48 | light[3].write(0); |
rixonfletcher | 2:b8a033375c27 | 49 | spinner=0; |
rixonfletcher | 2:b8a033375c27 | 50 | plunger=0; |
rixonfletcher | 2:b8a033375c27 | 51 | |
rixonfletcher | 2:b8a033375c27 | 52 | // initialize turret settings |
rixonfletcher | 2:b8a033375c27 | 53 | t.reset(); |
rixonfletcher | 2:b8a033375c27 | 54 | enc.reset(); |
rixonfletcher | 2:b8a033375c27 | 55 | m.speed(0.0); |
rixonfletcher | 2:b8a033375c27 | 56 | |
rixonfletcher | 2:b8a033375c27 | 57 | pc.printf("enter an angle\r\n"); |
rixonfletcher | 2:b8a033375c27 | 58 | pc.scanf("%f", &x); //angle input from matlab |
rixonfletcher | 2:b8a033375c27 | 59 | |
rixonfletcher | 2:b8a033375c27 | 60 | while(1) { |
rixonfletcher | 2:b8a033375c27 | 61 | |
rixonfletcher | 1:758c16346f28 | 62 | |
rixonfletcher | 2:b8a033375c27 | 63 | // Turret Actuation |
rixonfletcher | 2:b8a033375c27 | 64 | |
rixonfletcher | 2:b8a033375c27 | 65 | //turn to angle if target is detected |
rixonfletcher | 2:b8a033375c27 | 66 | t.reset(); |
rixonfletcher | 2:b8a033375c27 | 67 | while(old_t<5.0) { |
rixonfletcher | 2:b8a033375c27 | 68 | t.start(); |
rixonfletcher | 2:b8a033375c27 | 69 | wait(1/logfreq); |
rixonfletcher | 2:b8a033375c27 | 70 | //if(t.read()-old_t>=Ts) |
rixonfletcher | 1:758c16346f28 | 71 | |
rixonfletcher | 2:b8a033375c27 | 72 | count = enc.getPulses() ; |
rixonfletcher | 2:b8a033375c27 | 73 | theta = -count*((2*pi)/3200) ; |
rixonfletcher | 2:b8a033375c27 | 74 | // Calculate position error |
rixonfletcher | 2:b8a033375c27 | 75 | E=(x*(PI/180))-theta; |
rixonfletcher | 2:b8a033375c27 | 76 | |
rixonfletcher | 2:b8a033375c27 | 77 | // Control algorithm |
rixonfletcher | 2:b8a033375c27 | 78 | DC = (0.9841*old_DC) + (0.2348*E) - (0.2335*old_E); |
rixonfletcher | 2:b8a033375c27 | 79 | old_E = E; |
caleblegis | 3:4dcf81584894 | 80 | // m.speed(DC+0.15); |
rixonfletcher | 2:b8a033375c27 | 81 | old_DC = DC; |
rixonfletcher | 1:758c16346f28 | 82 | |
rixonfletcher | 2:b8a033375c27 | 83 | if(DC>0 && DC<0.7) |
caleblegis | 3:4dcf81584894 | 84 | m.speed(DC+0.4); |
caleblegis | 3:4dcf81584894 | 85 | if(E<-1.0) |
rixonfletcher | 2:b8a033375c27 | 86 | m.speed(DC+0.15); |
rixonfletcher | 2:b8a033375c27 | 87 | if (E<0.015 && E>-0.01) |
rixonfletcher | 2:b8a033375c27 | 88 | m.speed(0.0); |
rixonfletcher | 2:b8a033375c27 | 89 | else if(DC<0 && DC>-0.7) { |
caleblegis | 3:4dcf81584894 | 90 | m.speed(DC-0.8); |
rixonfletcher | 2:b8a033375c27 | 91 | if(E>-1.0) |
rixonfletcher | 2:b8a033375c27 | 92 | m.speed(DC-0.5); |
rixonfletcher | 2:b8a033375c27 | 93 | if (E>-0.015&& E<0.01) |
rixonfletcher | 2:b8a033375c27 | 94 | m.speed(0.0); |
rixonfletcher | 2:b8a033375c27 | 95 | } |
rixonfletcher | 2:b8a033375c27 | 96 | // Age variables |
rixonfletcher | 2:b8a033375c27 | 97 | old_t = t.read(); |
rixonfletcher | 2:b8a033375c27 | 98 | printf("time: %f angle: %f error: %f DC: %f\n\r", t.read(), theta, E,DC); |
rixonfletcher | 2:b8a033375c27 | 99 | |
rixonfletcher | 2:b8a033375c27 | 100 | }// end turret actuation |
rixonfletcher | 2:b8a033375c27 | 101 | m.speed(0.0); |
rixonfletcher | 2:b8a033375c27 | 102 | old_t=0.0; |
rixonfletcher | 2:b8a033375c27 | 103 | |
rixonfletcher | 2:b8a033375c27 | 104 | // shoots only when magazine is full |
rixonfletcher | 2:b8a033375c27 | 105 | |
rixonfletcher | 2:b8a033375c27 | 106 | |
rixonfletcher | 2:b8a033375c27 | 107 | |
rixonfletcher | 2:b8a033375c27 | 108 | //printf("Ready to fire, %d discs left. Type number of shots. \n \r\r",disc); |
rixonfletcher | 2:b8a033375c27 | 109 | |
rixonfletcher | 1:758c16346f28 | 110 | |
rixonfletcher | 2:b8a033375c27 | 111 | |
rixonfletcher | 2:b8a033375c27 | 112 | fwait=fire*shot_time; |
rixonfletcher | 2:b8a033375c27 | 113 | //printf("fire=%d \n\r",fire); |
rixonfletcher | 2:b8a033375c27 | 114 | //printf("fwait=%f \n\r",fwait); |
rixonfletcher | 2:b8a033375c27 | 115 | |
rixonfletcher | 2:b8a033375c27 | 116 | light[0].write(1); |
rixonfletcher | 2:b8a033375c27 | 117 | spinner=1; //spinner turns on for 3 seconds |
rixonfletcher | 2:b8a033375c27 | 118 | wait(1.5); |
rixonfletcher | 2:b8a033375c27 | 119 | |
rixonfletcher | 2:b8a033375c27 | 120 | light[3].write(1); |
rixonfletcher | 2:b8a033375c27 | 121 | plunger=1; //plunger turns on for desired number of shots |
rixonfletcher | 2:b8a033375c27 | 122 | wait(fwait); |
rixonfletcher | 2:b8a033375c27 | 123 | |
rixonfletcher | 2:b8a033375c27 | 124 | //calculate number of shots left |
rixonfletcher | 2:b8a033375c27 | 125 | //disc=disc-fire; |
rixonfletcher | 2:b8a033375c27 | 126 | //reset fire sequence |
rixonfletcher | 2:b8a033375c27 | 127 | |
rixonfletcher | 2:b8a033375c27 | 128 | |
rixonfletcher | 2:b8a033375c27 | 129 | //end of firing while loop |
rixonfletcher | 2:b8a033375c27 | 130 | |
rixonfletcher | 2:b8a033375c27 | 131 | |
rixonfletcher | 2:b8a033375c27 | 132 | light[0].write(0); |
rixonfletcher | 2:b8a033375c27 | 133 | light[3].write(0); |
rixonfletcher | 2:b8a033375c27 | 134 | spinner=0; |
rixonfletcher | 2:b8a033375c27 | 135 | plunger=0; |
rixonfletcher | 2:b8a033375c27 | 136 | |
rixonfletcher | 2:b8a033375c27 | 137 | |
rixonfletcher | 2:b8a033375c27 | 138 | m.speed(0.0); |
rixonfletcher | 2:b8a033375c27 | 139 | old_t=0.0; |
rixonfletcher | 2:b8a033375c27 | 140 | //enc.reset(); |
rixonfletcher | 2:b8a033375c27 | 141 | pc.printf("enter an angle\r\n"); |
rixonfletcher | 2:b8a033375c27 | 142 | pc.scanf("%f", &x); |
rixonfletcher | 2:b8a033375c27 | 143 | |
rixonfletcher | 2:b8a033375c27 | 144 | } //end of main while loop |
rixonfletcher | 2:b8a033375c27 | 145 | |
rixonfletcher | 2:b8a033375c27 | 146 | } //end main |