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