Diff: Computer_Vision_Firing.cpp

Revision:

0:ca9646805f61

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Computer_Vision_Firing.cpp	Tue Apr 28 13:17:51 2015 +0000
@@ -0,0 +1,98 @@
+#include "mbed.h"
+#include "QEI.h"
+
+PwmOut turret_speed(p21);
+DigitalOut turret_direction(p22);
+DigitalOut spinner[2] = {p5,LED1};
+DigitalOut plunger[2]={p6,LED2};
+
+Serial pc(USBTX, USBRX);
+
+QEI myEncoder (p16,p15, NC, 1600); 
+Timer t;
+
+int main()
+{
+//Turret Moving Variables
+float duty_cycle[500] = {0}; 
+float error[500] = {0}; 
+int pulses = 0; 
+float time[500]={0}; 
+float theta[500]={0}; 
+float ang_v[500]={0};
+//Moving Iteration Variable
+int k = 1; 
+//Start Variable
+char start_key; 
+//Firing Variables
+spinner[0] = 0; 
+spinner[1] = 0; 
+plunger[0] = 0; 
+plunger[1] = 0; 
+int clip = 17; // number of shots in the clip
+//Firing Iteration Variables
+int spinner_tracker = 1; 
+int plunger_tracker = 1; 
+int a = 1; 
+
+turret_speed = 0; 
+turret_direction = 1; 
+  
+while(clip>0)
+{
+    //Section to receive desired angle calculated from MATLAB
+    float theta_des = 1000.0;
+    pc.scanf("%f",&theta_des); //set theta des from MATLAB calculation sent to Mbed over Serial comms
+ 
+    pc.printf("Received %f\n",theta_des);
+    
+    error[0]= theta_des - theta[0]; 
+    pc.printf("Error calculated %f\n", error[0]); 
+  }  
+    /*wait(2); 
+ 
+    t.start();
+    for(k=1; k<500; k++) 
+    //while(fabs(error[k-1])>=0.02)
+        {
+        wait(0.01); 
+        pulses = myEncoder.getPulses(); 
+        theta[k] = ((float)pulses/ (1600.0*2.0))*(-2.0*3.14);
+        time[k] = t.read();
+        ang_v[k]=(theta[k]-theta[k-1])/(time[k]-time[k-1]);    
+        error[k] = theta_des - theta[k];  
+        duty_cycle[k]=0.9685*duty_cycle[k-1] +0.2552*error[k]-0.2527*error[k-1];
+            if(duty_cycle[k]<0)
+            {
+                turret_direction = 0;
+            }
+            else
+            {
+                turret_direction = 1;
+            }
+        turret_speed = fabs(duty_cycle[k])+0.07; 
+        k = k+1; 
+        }  
+    turret_speed = 0; 
+    t.stop();    
+    //Firing gun, shoot 5 shots
+    for(a=1; a<6; a++)
+        {
+            spinner[0] = 1; //turn spinner motor on
+            spinner[1] =1;//turn spinner LED on, as a check
+             
+            plunger[0] = 1; //turn plunger motor on, start firing
+            plunger[1] = 1; //turn plunger LED on, as a check
+            
+            float time = 5*0.25; 
+            wait(time); //run the plunger motor while firing off these 5 shots
+            
+            plunger[0] = 0; //turn off plunger motor, stop firing
+            plunger[1] =0; //turn off plunger LED
+            
+            clip = clip -5; 
+        }  
+    //Stop Firing Gun
+     
+    }*/
+}