ES305H Lanier Turchan / Mbed 2 deprecated Computer_Vision_Firing

final part code

Dependencies:   QEI mbed

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?

UserRevisionLine numberNew 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 }