ES309 Project Team / Mbed 2 deprecated ES309_NerfTurretfinal1

454

Dependencies:   Motor QEI mbed mbedWSEsbc

Fork of ES309_NerfTurretfinal by ES309 Project Team

main.cpp@4:806d9d971c26, 2016-05-12 (annotated)

Committer:
caleblegis
Date:
Thu May 12 14:04:09 2016 +0000
Revision:
4:806d9d971c26
Parent:
3:4dcf81584894 
h;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rixonfletcher 1:758c16346f28 1 #include "mbed.h"
rixonfletcher 2:b8a033375c27 2 #include "Motor.h"
rixonfletcher 2:b8a033375c27 3 #include "QEI.h"
rixonfletcher 2:b8a033375c27 4 #define pi (3.14159265)
rixonfletcher 2:b8a033375c27 5 #define logfreq (100)
rixonfletcher 2:b8a033375c27 6 #define data_pts (500)
rixonfletcher 2:b8a033375c27 7
rixonfletcher 2:b8a033375c27 8 Serial pc(USBTX, USBRX);
rixonfletcher 2:b8a033375c27 9 Motor m(p25, p23, p27);
rixonfletcher 2:b8a033375c27 10 QEI enc(p16,p17,NC,1600);
rixonfletcher 1:758c16346f28 11
rixonfletcher 1:758c16346f28 12 DigitalOut light[]= {LED1, LED2, LED3, LED4};
rixonfletcher 2:b8a033375c27 13 DigitalOut spinner(p21);
rixonfletcher 2:b8a033375c27 14 DigitalOut plunger(p22);
rixonfletcher 2:b8a033375c27 15
rixonfletcher 2:b8a033375c27 16 // firing circuit configuration
rixonfletcher 2:b8a033375c27 17 int disc=20; //number of discs in magazine
rixonfletcher 2:b8a033375c27 18 float shot_time=0.6; //time per shot. 3.51s for 5 shots
rixonfletcher 2:b8a033375c27 19 int fire=4; //number of bullets to fire (reset to zero after every round of shots)
rixonfletcher 2:b8a033375c27 20 float fwait; //fire time wait
rixonfletcher 2:b8a033375c27 21 int shoot;
rixonfletcher 2:b8a033375c27 22
rixonfletcher 2:b8a033375c27 23
rixonfletcher 2:b8a033375c27 24 //turret circuit configuration
rixonfletcher 2:b8a033375c27 25 float motspeed;
rixonfletcher 2:b8a033375c27 26 float x;
rixonfletcher 2:b8a033375c27 27 float rad;
rixonfletcher 2:b8a033375c27 28 int count;
rixonfletcher 2:b8a033375c27 29 Timer t;
rixonfletcher 2:b8a033375c27 30 float rad_one=0;
rixonfletcher 2:b8a033375c27 31 float time_one=0;
rixonfletcher 2:b8a033375c27 32 float omega; //angular velocity
rixonfletcher 2:b8a033375c27 33 float old_E;
rixonfletcher 2:b8a033375c27 34 float E; // error
rixonfletcher 2:b8a033375c27 35 float PI=3.14159265;
rixonfletcher 2:b8a033375c27 36 float old_t;
rixonfletcher 2:b8a033375c27 37 float DC;
rixonfletcher 2:b8a033375c27 38 float old_DC;
rixonfletcher 2:b8a033375c27 39 float theta;
rixonfletcher 2:b8a033375c27 40 //float theta_desired=x;
rixonfletcher 2:b8a033375c27 41 float Ts=0.01;
rixonfletcher 2:b8a033375c27 42
rixonfletcher 1:758c16346f28 43
rixonfletcher 1:758c16346f28 44 int main()
rixonfletcher 1:758c16346f28 45 {
rixonfletcher 2:b8a033375c27 46 // initialize firing settings
rixonfletcher 2:b8a033375c27 47 light[0].write(0);
rixonfletcher 2:b8a033375c27 48 light[3].write(0);
rixonfletcher 2:b8a033375c27 49 spinner=0;
rixonfletcher 2:b8a033375c27 50 plunger=0;
rixonfletcher 2:b8a033375c27 51
rixonfletcher 2:b8a033375c27 52 // initialize turret settings
rixonfletcher 2:b8a033375c27 53 t.reset();
rixonfletcher 2:b8a033375c27 54 enc.reset();
rixonfletcher 2:b8a033375c27 55 m.speed(0.0);
rixonfletcher 2:b8a033375c27 56
rixonfletcher 2:b8a033375c27 57 pc.printf("enter an angle\r\n");
rixonfletcher 2:b8a033375c27 58 pc.scanf("%f", &x); //angle input from matlab
rixonfletcher 2:b8a033375c27 59
rixonfletcher 2:b8a033375c27 60 while(1) {
rixonfletcher 2:b8a033375c27 61
rixonfletcher 1:758c16346f28 62
rixonfletcher 2:b8a033375c27 63 // Turret Actuation
rixonfletcher 2:b8a033375c27 64
rixonfletcher 2:b8a033375c27 65 //turn to angle if target is detected
rixonfletcher 2:b8a033375c27 66 t.reset();
rixonfletcher 2:b8a033375c27 67 while(old_t<5.0) {
rixonfletcher 2:b8a033375c27 68 t.start();
rixonfletcher 2:b8a033375c27 69 wait(1/logfreq);
rixonfletcher 2:b8a033375c27 70 //if(t.read()-old_t>=Ts)
rixonfletcher 1:758c16346f28 71
rixonfletcher 2:b8a033375c27 72 count = enc.getPulses() ;
rixonfletcher 2:b8a033375c27 73 theta = -count*((2*pi)/3200) ;
rixonfletcher 2:b8a033375c27 74 // Calculate position error
rixonfletcher 2:b8a033375c27 75 E=(x*(PI/180))-theta;
rixonfletcher 2:b8a033375c27 76
rixonfletcher 2:b8a033375c27 77 // Control algorithm
rixonfletcher 2:b8a033375c27 78 DC = (0.9841*old_DC) + (0.2348*E) - (0.2335*old_E);
rixonfletcher 2:b8a033375c27 79 old_E = E;
caleblegis 3:4dcf81584894 80 // m.speed(DC+0.15);
rixonfletcher 2:b8a033375c27 81 old_DC = DC;
rixonfletcher 1:758c16346f28 82
rixonfletcher 2:b8a033375c27 83 if(DC>0 && DC<0.7)
caleblegis 3:4dcf81584894 84 m.speed(DC+0.4);
caleblegis 3:4dcf81584894 85 if(E<-1.0)
rixonfletcher 2:b8a033375c27 86 m.speed(DC+0.15);
rixonfletcher 2:b8a033375c27 87 if (E<0.015 && E>-0.01)
rixonfletcher 2:b8a033375c27 88 m.speed(0.0);
rixonfletcher 2:b8a033375c27 89 else if(DC<0 && DC>-0.7) {
caleblegis 4:806d9d971c26 90 m.speed(DC-0.7);
rixonfletcher 2:b8a033375c27 91 if(E>-1.0)
rixonfletcher 2:b8a033375c27 92 m.speed(DC-0.5);
rixonfletcher 2:b8a033375c27 93 if (E>-0.015&& E<0.01)
rixonfletcher 2:b8a033375c27 94 m.speed(0.0);
rixonfletcher 2:b8a033375c27 95 }
rixonfletcher 2:b8a033375c27 96 // Age variables
rixonfletcher 2:b8a033375c27 97 old_t = t.read();
rixonfletcher 2:b8a033375c27 98 printf("time: %f angle: %f error: %f DC: %f\n\r", t.read(), theta, E,DC);
rixonfletcher 2:b8a033375c27 99
rixonfletcher 2:b8a033375c27 100 }// end turret actuation
rixonfletcher 2:b8a033375c27 101 m.speed(0.0);
rixonfletcher 2:b8a033375c27 102 old_t=0.0;
rixonfletcher 2:b8a033375c27 103
rixonfletcher 2:b8a033375c27 104 // shoots only when magazine is full
rixonfletcher 2:b8a033375c27 105
rixonfletcher 2:b8a033375c27 106
rixonfletcher 2:b8a033375c27 107
rixonfletcher 2:b8a033375c27 108 //printf("Ready to fire, %d discs left. Type number of shots. \n \r\r",disc);
rixonfletcher 2:b8a033375c27 109
rixonfletcher 1:758c16346f28 110
rixonfletcher 2:b8a033375c27 111
rixonfletcher 2:b8a033375c27 112 fwait=fire*shot_time;
rixonfletcher 2:b8a033375c27 113 //printf("fire=%d \n\r",fire);
rixonfletcher 2:b8a033375c27 114 //printf("fwait=%f \n\r",fwait);
rixonfletcher 2:b8a033375c27 115
rixonfletcher 2:b8a033375c27 116 light[0].write(1);
rixonfletcher 2:b8a033375c27 117 spinner=1; //spinner turns on for 3 seconds
rixonfletcher 2:b8a033375c27 118 wait(1.5);
rixonfletcher 2:b8a033375c27 119
rixonfletcher 2:b8a033375c27 120 light[3].write(1);
rixonfletcher 2:b8a033375c27 121 plunger=1; //plunger turns on for desired number of shots
rixonfletcher 2:b8a033375c27 122 wait(fwait);
rixonfletcher 2:b8a033375c27 123
rixonfletcher 2:b8a033375c27 124 //calculate number of shots left
rixonfletcher 2:b8a033375c27 125 //disc=disc-fire;
rixonfletcher 2:b8a033375c27 126 //reset fire sequence
rixonfletcher 2:b8a033375c27 127
rixonfletcher 2:b8a033375c27 128
rixonfletcher 2:b8a033375c27 129 //end of firing while loop
rixonfletcher 2:b8a033375c27 130
rixonfletcher 2:b8a033375c27 131
rixonfletcher 2:b8a033375c27 132 light[0].write(0);
rixonfletcher 2:b8a033375c27 133 light[3].write(0);
rixonfletcher 2:b8a033375c27 134 spinner=0;
rixonfletcher 2:b8a033375c27 135 plunger=0;
rixonfletcher 2:b8a033375c27 136
rixonfletcher 2:b8a033375c27 137
rixonfletcher 2:b8a033375c27 138 m.speed(0.0);
rixonfletcher 2:b8a033375c27 139 old_t=0.0;
rixonfletcher 2:b8a033375c27 140 //enc.reset();
rixonfletcher 2:b8a033375c27 141 pc.printf("enter an angle\r\n");
rixonfletcher 2:b8a033375c27 142 pc.scanf("%f", &x);
rixonfletcher 2:b8a033375c27 143
rixonfletcher 2:b8a033375c27 144 } //end of main while loop
rixonfletcher 2:b8a033375c27 145
rixonfletcher 2:b8a033375c27 146 } //end main