ES305H Lanier Turchan
/
Computer_Vision_Firing
final part code
Computer_Vision_Firing.cpp@0:ca9646805f61, 2015-04-28 (annotated)
- Committer:
- LanierUSNA16
- Date:
- Tue Apr 28 13:17:51 2015 +0000
- Revision:
- 0:ca9646805f61
Code for final part;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LanierUSNA16 | 0:ca9646805f61 | 1 | #include "mbed.h" |
LanierUSNA16 | 0:ca9646805f61 | 2 | #include "QEI.h" |
LanierUSNA16 | 0:ca9646805f61 | 3 | |
LanierUSNA16 | 0:ca9646805f61 | 4 | PwmOut turret_speed(p21); |
LanierUSNA16 | 0:ca9646805f61 | 5 | DigitalOut turret_direction(p22); |
LanierUSNA16 | 0:ca9646805f61 | 6 | DigitalOut spinner[2] = {p5,LED1}; |
LanierUSNA16 | 0:ca9646805f61 | 7 | DigitalOut plunger[2]={p6,LED2}; |
LanierUSNA16 | 0:ca9646805f61 | 8 | |
LanierUSNA16 | 0:ca9646805f61 | 9 | Serial pc(USBTX, USBRX); |
LanierUSNA16 | 0:ca9646805f61 | 10 | |
LanierUSNA16 | 0:ca9646805f61 | 11 | QEI myEncoder (p16,p15, NC, 1600); |
LanierUSNA16 | 0:ca9646805f61 | 12 | Timer t; |
LanierUSNA16 | 0:ca9646805f61 | 13 | |
LanierUSNA16 | 0:ca9646805f61 | 14 | int main() |
LanierUSNA16 | 0:ca9646805f61 | 15 | { |
LanierUSNA16 | 0:ca9646805f61 | 16 | //Turret Moving Variables |
LanierUSNA16 | 0:ca9646805f61 | 17 | float duty_cycle[500] = {0}; |
LanierUSNA16 | 0:ca9646805f61 | 18 | float error[500] = {0}; |
LanierUSNA16 | 0:ca9646805f61 | 19 | int pulses = 0; |
LanierUSNA16 | 0:ca9646805f61 | 20 | float time[500]={0}; |
LanierUSNA16 | 0:ca9646805f61 | 21 | float theta[500]={0}; |
LanierUSNA16 | 0:ca9646805f61 | 22 | float ang_v[500]={0}; |
LanierUSNA16 | 0:ca9646805f61 | 23 | //Moving Iteration Variable |
LanierUSNA16 | 0:ca9646805f61 | 24 | int k = 1; |
LanierUSNA16 | 0:ca9646805f61 | 25 | //Start Variable |
LanierUSNA16 | 0:ca9646805f61 | 26 | char start_key; |
LanierUSNA16 | 0:ca9646805f61 | 27 | //Firing Variables |
LanierUSNA16 | 0:ca9646805f61 | 28 | spinner[0] = 0; |
LanierUSNA16 | 0:ca9646805f61 | 29 | spinner[1] = 0; |
LanierUSNA16 | 0:ca9646805f61 | 30 | plunger[0] = 0; |
LanierUSNA16 | 0:ca9646805f61 | 31 | plunger[1] = 0; |
LanierUSNA16 | 0:ca9646805f61 | 32 | int clip = 17; // number of shots in the clip |
LanierUSNA16 | 0:ca9646805f61 | 33 | //Firing Iteration Variables |
LanierUSNA16 | 0:ca9646805f61 | 34 | int spinner_tracker = 1; |
LanierUSNA16 | 0:ca9646805f61 | 35 | int plunger_tracker = 1; |
LanierUSNA16 | 0:ca9646805f61 | 36 | int a = 1; |
LanierUSNA16 | 0:ca9646805f61 | 37 | |
LanierUSNA16 | 0:ca9646805f61 | 38 | turret_speed = 0; |
LanierUSNA16 | 0:ca9646805f61 | 39 | turret_direction = 1; |
LanierUSNA16 | 0:ca9646805f61 | 40 | |
LanierUSNA16 | 0:ca9646805f61 | 41 | while(clip>0) |
LanierUSNA16 | 0:ca9646805f61 | 42 | { |
LanierUSNA16 | 0:ca9646805f61 | 43 | //Section to receive desired angle calculated from MATLAB |
LanierUSNA16 | 0:ca9646805f61 | 44 | float theta_des = 1000.0; |
LanierUSNA16 | 0:ca9646805f61 | 45 | pc.scanf("%f",&theta_des); //set theta des from MATLAB calculation sent to Mbed over Serial comms |
LanierUSNA16 | 0:ca9646805f61 | 46 | |
LanierUSNA16 | 0:ca9646805f61 | 47 | pc.printf("Received %f\n",theta_des); |
LanierUSNA16 | 0:ca9646805f61 | 48 | |
LanierUSNA16 | 0:ca9646805f61 | 49 | error[0]= theta_des - theta[0]; |
LanierUSNA16 | 0:ca9646805f61 | 50 | pc.printf("Error calculated %f\n", error[0]); |
LanierUSNA16 | 0:ca9646805f61 | 51 | } |
LanierUSNA16 | 0:ca9646805f61 | 52 | /*wait(2); |
LanierUSNA16 | 0:ca9646805f61 | 53 | |
LanierUSNA16 | 0:ca9646805f61 | 54 | t.start(); |
LanierUSNA16 | 0:ca9646805f61 | 55 | for(k=1; k<500; k++) |
LanierUSNA16 | 0:ca9646805f61 | 56 | //while(fabs(error[k-1])>=0.02) |
LanierUSNA16 | 0:ca9646805f61 | 57 | { |
LanierUSNA16 | 0:ca9646805f61 | 58 | wait(0.01); |
LanierUSNA16 | 0:ca9646805f61 | 59 | pulses = myEncoder.getPulses(); |
LanierUSNA16 | 0:ca9646805f61 | 60 | theta[k] = ((float)pulses/ (1600.0*2.0))*(-2.0*3.14); |
LanierUSNA16 | 0:ca9646805f61 | 61 | time[k] = t.read(); |
LanierUSNA16 | 0:ca9646805f61 | 62 | ang_v[k]=(theta[k]-theta[k-1])/(time[k]-time[k-1]); |
LanierUSNA16 | 0:ca9646805f61 | 63 | error[k] = theta_des - theta[k]; |
LanierUSNA16 | 0:ca9646805f61 | 64 | duty_cycle[k]=0.9685*duty_cycle[k-1] +0.2552*error[k]-0.2527*error[k-1]; |
LanierUSNA16 | 0:ca9646805f61 | 65 | if(duty_cycle[k]<0) |
LanierUSNA16 | 0:ca9646805f61 | 66 | { |
LanierUSNA16 | 0:ca9646805f61 | 67 | turret_direction = 0; |
LanierUSNA16 | 0:ca9646805f61 | 68 | } |
LanierUSNA16 | 0:ca9646805f61 | 69 | else |
LanierUSNA16 | 0:ca9646805f61 | 70 | { |
LanierUSNA16 | 0:ca9646805f61 | 71 | turret_direction = 1; |
LanierUSNA16 | 0:ca9646805f61 | 72 | } |
LanierUSNA16 | 0:ca9646805f61 | 73 | turret_speed = fabs(duty_cycle[k])+0.07; |
LanierUSNA16 | 0:ca9646805f61 | 74 | k = k+1; |
LanierUSNA16 | 0:ca9646805f61 | 75 | } |
LanierUSNA16 | 0:ca9646805f61 | 76 | turret_speed = 0; |
LanierUSNA16 | 0:ca9646805f61 | 77 | t.stop(); |
LanierUSNA16 | 0:ca9646805f61 | 78 | //Firing gun, shoot 5 shots |
LanierUSNA16 | 0:ca9646805f61 | 79 | for(a=1; a<6; a++) |
LanierUSNA16 | 0:ca9646805f61 | 80 | { |
LanierUSNA16 | 0:ca9646805f61 | 81 | spinner[0] = 1; //turn spinner motor on |
LanierUSNA16 | 0:ca9646805f61 | 82 | spinner[1] =1;//turn spinner LED on, as a check |
LanierUSNA16 | 0:ca9646805f61 | 83 | |
LanierUSNA16 | 0:ca9646805f61 | 84 | plunger[0] = 1; //turn plunger motor on, start firing |
LanierUSNA16 | 0:ca9646805f61 | 85 | plunger[1] = 1; //turn plunger LED on, as a check |
LanierUSNA16 | 0:ca9646805f61 | 86 | |
LanierUSNA16 | 0:ca9646805f61 | 87 | float time = 5*0.25; |
LanierUSNA16 | 0:ca9646805f61 | 88 | wait(time); //run the plunger motor while firing off these 5 shots |
LanierUSNA16 | 0:ca9646805f61 | 89 | |
LanierUSNA16 | 0:ca9646805f61 | 90 | plunger[0] = 0; //turn off plunger motor, stop firing |
LanierUSNA16 | 0:ca9646805f61 | 91 | plunger[1] =0; //turn off plunger LED |
LanierUSNA16 | 0:ca9646805f61 | 92 | |
LanierUSNA16 | 0:ca9646805f61 | 93 | clip = clip -5; |
LanierUSNA16 | 0:ca9646805f61 | 94 | } |
LanierUSNA16 | 0:ca9646805f61 | 95 | //Stop Firing Gun |
LanierUSNA16 | 0:ca9646805f61 | 96 | |
LanierUSNA16 | 0:ca9646805f61 | 97 | }*/ |
LanierUSNA16 | 0:ca9646805f61 | 98 | } |