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