gf
Dependencies: Motor QEI mbed mbedWSEsbc
Fork of ES309_NerfTurret by
main.cpp
- Committer:
- rixonfletcher
- Date:
- 2016-05-12
- Revision:
- 2:b8a033375c27
- Parent:
- 1:758c16346f28
- Child:
- 3:4dcf81584894
File content as of revision 2:b8a033375c27:
#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(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() { // 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) { // 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) 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; 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); 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