gf
Dependencies: Motor QEI mbed mbedWSEsbc
Fork of ES309_NerfTurret by
Diff: main.cpp
- Revision:
- 2:b8a033375c27
- Parent:
- 1:758c16346f28
- Child:
- 3:4dcf81584894
--- a/main.cpp Tue Mar 01 15:00:31 2016 +0000 +++ b/main.cpp Thu May 12 12:51:37 2016 +0000 @@ -1,48 +1,146 @@ #include "mbed.h" +#include "Motor.h" +#include "QEI.h" +#define pi (3.14159265) +#define logfreq (100) +#define data_pts (500) + +Serial pc(USBTX, USBRX); +Motor m(p25, p23, p27); +QEI enc(p16,p17,NC,1600); DigitalOut light[]= {LED1, LED2, LED3, LED4}; -DigitalOut spinner(p20); -DigitalOut plunger(p21); +DigitalOut spinner(p21); +DigitalOut plunger(p22); + +// firing circuit configuration +int disc=20; //number of discs in magazine +float shot_time=0.6; //time per shot. 3.51s for 5 shots +int fire=4; //number of bullets to fire (reset to zero after every round of shots) +float fwait; //fire time wait +int shoot; + + +//turret circuit configuration +float motspeed; +float x; +float rad; +int count; +Timer t; +float rad_one=0; +float time_one=0; +float omega; //angular velocity +float old_E; +float E; // error +float PI=3.14159265; +float old_t; +float DC; +float old_DC; +float theta; +//float theta_desired=x; +float Ts=0.01; + int main() { - int disc=20; //number of discs in magazine - float shot_time=0.6; //time per shot. 3.51s for 5 shots - int fire=0; //number of bullets to fire (reset to zero after every round of shots) - float fwait; //fire time wait + // initialize firing settings + light[0].write(0); + light[3].write(0); + spinner=0; + plunger=0; + + // initialize turret settings + t.reset(); + enc.reset(); + m.speed(0.0); + + pc.printf("enter an angle\r\n"); + pc.scanf("%f", &x); //angle input from matlab + + while(1) { + - while(disc>0) { - light[0].write(0); - light[3].write(0); - spinner=0; - plunger=0; - printf("Ready to fire, %d discs left. Type number of shots. \n \r\r",disc); - scanf("%d",&fire); + // Turret Actuation + + //turn to angle if target is detected + t.reset(); + while(old_t<5.0) { + t.start(); + wait(1/logfreq); + //if(t.read()-old_t>=Ts) - if (fire>0) { //fires for desired number of shots + count = enc.getPulses() ; + theta = -count*((2*pi)/3200) ; + // Calculate position error + E=(x*(PI/180))-theta; + + // Control algorithm + DC = (0.9841*old_DC) + (0.2348*E) - (0.2335*old_E); + old_E = E; + m.speed(DC+0.15); + old_DC = DC; - fwait=fire*shot_time; - printf("fire=%d \n\r",fire); - printf("fwait=%f \n\r",fwait); - - light[0].write(1); - spinner=1; //spinner turns on for 3 seconds - wait(1.5); - - light[3].write(1); - plunger=1; //plunger turns on for desired number of shots - wait(fwait); + if(DC>0 && DC<0.7) + m.speed(DC+0.1); + if(E>-1.0) + m.speed(DC+0.15); + if (E<0.015 && E>-0.01) + m.speed(0.0); + else if(DC<0 && DC>-0.7) { + m.speed(DC-0.34); + if(E>-1.0) + m.speed(DC-0.5); + if (E>-0.015&& E<0.01) + m.speed(0.0); + } + // Age variables + old_t = t.read(); + printf("time: %f angle: %f error: %f DC: %f\n\r", t.read(), theta, E,DC); + + }// end turret actuation + m.speed(0.0); + old_t=0.0; + +// shoots only when magazine is full + + + +//printf("Ready to fire, %d discs left. Type number of shots. \n \r\r",disc); + - //calculate number of shots left - disc=disc-fire; - //reset fire sequence - fire=0; - fwait=0; - } - } - light[0].write(0); - light[3].write(0); - spinner=0; - plunger=0; - printf("You ran out of discs. Press the reset button to reload. \n\r\r"); -} \ No newline at end of file + + fwait=fire*shot_time; + //printf("fire=%d \n\r",fire); + //printf("fwait=%f \n\r",fwait); + + light[0].write(1); + spinner=1; //spinner turns on for 3 seconds + wait(1.5); + + light[3].write(1); + plunger=1; //plunger turns on for desired number of shots + wait(fwait); + + //calculate number of shots left + //disc=disc-fire; + //reset fire sequence + + + //end of firing while loop + + + light[0].write(0); + light[3].write(0); + spinner=0; + plunger=0; + + + m.speed(0.0); + old_t=0.0; + //enc.reset(); + pc.printf("enter an angle\r\n"); + pc.scanf("%f", &x); + + } //end of main while loop + +} //end main \ No newline at end of file