Revision 2:825571dce613, committed 2019-04-23

Comitter:

LukeMar

Date:

Tue Apr 23 17:52:27 2019 +0000

Parent:

1:4b4d5d18fc57

Commit message:

4/23/19

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Tue Apr 23 17:52:27 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- a/main.cpp	Thu Mar 14 22:09:48 2019 +0000
+++ b/main.cpp	Tue Apr 23 17:52:27 2019 +0000
@@ -17,340 +17,155 @@
 //control thread is a work in progress
 
 #include "mbed.h"
-#include "MBed_Adafruit_GPS.h"
-#include "BNO055.h"
-#include "Spektrum.h"
-#include "unity.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <QEI.h>
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut spin(p20);
+DigitalOut fire(p19);
+PwmOut pwm(p25);
+DigitalOut dir(p27);
+Serial pc(USBTX,USBRX);
+QEI turret(p17, p18, NC, 1600);
+Timer t;
 
-Serial pc(USBTX, USBRX, 115200);
-Spektrum rx(p13, p14);
+int qq;
+float pulses;
+float revs;
+float dc;
+float theta;
+float err = 0.0;
+float err_old = 0.0;
+float err_ancient = 0.0;
+float dc_old = 0.0;
+float dc_ancient = 0.0;
+float des_theta=0.0;
+float t_now;
+int ii=0;
+int shots;
 int i;
-int j;
-float now;
-//***Mast universal variables***
-AnalogIn   ain(p20);
-PwmOut  motor( p25 );
-DigitalOut   dir( p24 );
-DigitalOut    I(p26);
-DigitalOut slp(p29); //sleep
-float d_ang = 180.0;
-float pos = 180.0;
-float RC_O;
-//***rudder universal variables***
-AnalogIn   r_ain(p15);
-PwmOut  rudder( p22 );
-DigitalOut   r_dir( p21 );
-DigitalOut    r_I(p23);
-DigitalOut r_slp(p30); //sleep
-float r_ang;
-float r_pos;
-float RC_1;
+
+//float t;
+//float t_2;
+int yes;
+
+Ticker Controller;
+void CtrCode()
+{
+    pulses = turret.getPulses();
+    revs = -pulses / 3200.0;
+    theta = revs*360.0;
+
+    err = des_theta-theta;
+
+    //dc = 1.9703*dc_old - 0.9703*dc_ancient + .1471*err- 0.293*err_old + 0.146*err_ancient;    //1.9703*dc_old - 0.9703*dc_ancient + .1471*err - 0.293*err_old + 0.146*err_ancient;
+    dc = 1.9703*dc_old - 0.9703*dc_ancient + 0.7918*err - 1.5762*err_old + 0.7844*err_ancient;
 
-float xx = 5.5; //changes the threshold that the motor goes to sleep on
-float gg = 9.5; //changes the threshold that the motor goes to sleep on for mast
-float zz = 121; //changes wait at end of rc thread
-float ww = 5; //changes wait time at end of if statments in mast rudder threads
-int ff = 65;//changes wait at end of telemetry
-//****universal variables for Telemetry****
-DigitalOut heartbeat(LED1);
-Serial * gps_Serial;
-BNO055 bno(p28, p27);
-char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
-float info; //a multiuse variable used for sending things over GPS
-Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
-const int refresh_Time = 2000; //refresh time in ms
-       //wait time between pieces of telemetry info
+    err_ancient = err_old;
+    err_old = err;
+    dc_ancient = dc_old;
+    dc_old = dc;
+
+    if (dc>0) {
+        dir = 1;
+    } else {
+        dir = 0;
+    }
+
+//Filter for derivative
+    if(t.read()<=3.0) {
+        pwm.write(abs(dc));
+    }
+    if(t.read()>3.0) {
+        if(abs(err)<=0.50) {
+            pwm.write(0);
+        } else {
+            pwm.write(abs(dc));
+        }
+    }
+}
 
 
-union Float {       //slightly mysterious data type that we use to send data over serial
-    float    m_float;
-    uint8_t  m_bytes[sizeof(float)];
-};
-float        data;
-uint8_t      bytes[sizeof(float)];
-Float        myFloat;
-
-//**get position**
-float posi();
-float posr();
-
-//*********read reads yaw from IMU **************
-void yaw_read()
-{
-    bno.get_angles();
-    data = bno.euler.yaw;
-    myFloat.m_float = data;
-    data = myFloat.m_float;   // get the float back from the union
-    pc.printf("%.1f", data);
-    Thread::wait(ff);
-
-}
-
-void roll_read()
+void FireCode()
 {
-    bno.get_angles();
-    data = bno.euler.roll;
-    myFloat.m_float = data;
-    data = myFloat.m_float;   // get the float back from the union
-    pc.printf("%.1f", data);
-    Thread::wait(ff);
-
-}
-
-//********concatenate time (not currently in use)**********
-float conc(float hr, float min, float sec )
-{
-    return hr+(min/60.0)+(sec/3600.0);
-}
-//********Initialize IMU************
+    
+    while(1) {
+        int counter = 0;
+        while(abs(err)<=0.50) {
+            Thread::wait(5);
+            ii++;
+            if(ii>15) {
 
-void bno_init(void)
-{
-    if(bno.check()) {
-        //pc.printf("BNO055 connected\r\n");
-        bno.reset();
-        Thread::wait(30);
-        bno.setmode(OPERATION_MODE_CONFIG);
-        bno.SetExternalCrystal(1);
-        //bno.set_orientation(1);
-        bno.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
-        //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF);   //no magnetometer
-        bno.set_angle_units(RADIANS);
-    } else {
-        //pc.printf("BNO055 NOT connected\r\n Program Trap.");
-        while(1);
+                spin = 1;
+                led1 = 1;
+                Thread::wait(3000);
+                
+                while(counter<shots) {
+                    fire = 1;
+                    led2 = 1;
+                    Thread::wait(100);
+                    led2 = 0;
+                    fire = 0;
+                    Thread::wait(7500);
+                    counter = counter + 1;
+
+                }
+                spin = 0;
+                led1 = 0;
+            }
+        }
     }
 }
 
 // *****threading*****
-Thread telemetry_thread;
-Thread mast_thread;
-Thread rudder_thread;
+Thread Spin_thread;
+Thread Shoot_thread;
 
-void telemetry_callback(void);
-void mast_callback(void);
-void rudder_callback(void);
+
+void Spin_callback(void);
+void Shoot_callback(void);
 
 int main()
 {
+
+    float pwm_now;
+
+    pc.baud(9600); // set baud rate
+    dir=1;
+    pwm.period(1.0/(20*10^3));
+    pc.scanf("%f,%d",&des_theta,&shots); //scan in desired theta and # of shots from Tera term
+    qq = 0;
+    while(qq<shots) {
+        led2 = 1;
+        Thread::wait(750);
+        led2 = 0;
+        qq++;
+    }
+    t.start();
     //start all threads
-    wait(1);
-    telemetry_thread.start(callback(telemetry_callback));
-    mast_thread.start(callback(mast_callback));
-    rudder_thread.start(callback(rudder_callback));
+    Thread::wait(1000);
+    Spin_thread.start(callback(Spin_callback));
+    Shoot_thread.start(callback(Shoot_callback));
+
+    while(t.read()<=25) {
+        t_now = t.read();
+        pc.printf("Time: %f, Error: %f, DC: %f\n", t_now,err, dc);
+    }
+
+    t.stop();
     //could have a heart beat but I tried to reduce the number of threads fighting for time
 }//end main
 
-float posi()
+void Spin_callback(void)
 {
-    float p1;
-    float p2;
-    float p3;
-    p1 = ain*449.0-147.53;  //ain*447.73-147.53;
-    Thread::wait(3);
-    p2 = ain*447.73-147.53;
-    Thread::wait(3);
-    p3 = ain*447.73-147.53;
-    
-    return (p1+p2+p3)/3.0;
-}
-float posr()
-{
-    float r1;
-    float r2;
-    float r3;
-    r1 = (r_ain-.108)/.002466;
-    Thread::wait(3);
-    r2 = (r_ain-.108)/.002466;
-    Thread::wait(3);
-    r3 = (r_ain-.108)/.002466;
-    return (r1+r2+r3)/3.0;
+
+    Controller.attach(&CtrCode,.0083);
 }
 
-void telemetry_callback(void)
+void Shoot_callback(void)
 {
-    //uint64_t now;
-    //debug("telemetry_thread started\r\n");
-
-    
-    gps_Serial = new Serial(p9,p10); //serial object for use w/ GPS
-    Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
-
-    myGPS.begin(9600);  //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
-    //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf
-
-    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
-    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
-    myGPS.sendCommand(PGCMD_ANTENNA);
-
-    //pc.printf("Connection established at 115200 baud...\n");
-    bno_init();
-
-    Thread::wait(100);
-    refresh_Timer.start();  //starts the clock on the timer
-
-    while(true) {
-
-        c = myGPS.read();   //queries the GPS
-        bno.get_angles();
-        //check if we recieved a new message from GPS, if so, attempt to parse it,
-        if ( myGPS.newNMEAreceived() ) {
-            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
-                continue;
-            }//if ( !myGPS.parse..
-
-            if (refresh_Timer.read_ms() >= refresh_Time) {
-                refresh_Timer.reset();
+    FireCode();
 
-               /* data = conc(myGPS.hour, myGPS.minute, myGPS.seconds );               // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
-                myFloat.m_float = data;
-                data = myFloat.m_float;   // get the float back from the union
-                pc.printf("%2.0f", data);
-                Thread::wait(ff);*/
-                //  this part can send hr min second separately, commented to save thread space
-                data = myGPS.hour;               // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
-                myFloat.m_float = data;
-                data = myFloat.m_float;   // get the float back from the union
-                pc.printf("%2.0f", data);
-                Thread::wait(35);
-                //send min
-                data = myGPS.minute;               // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
-                myFloat.m_float = data;
-                data = myFloat.m_float;   // get the float back from the union
-                pc.printf("%2.0f", data);
-                Thread::wait(35);
-                //send sec
-                data = myGPS.seconds;               // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
-                myFloat.m_float = data;
-                data = myFloat.m_float;   // get the float back from the union
-                pc.printf("%2.0f", data);
-                Thread::wait(35);
-                
-                if (myGPS.fix) {
-                    //rudder and mast position will log here
-                    //send mast angle
-                   /* data = posi();
-                    myFloat.m_float = data;
-                    data = myFloat.m_float;   // get the float back from the union
-                    pc.printf("%.1f", data);
-                    Thread::wait(ff);
-                    //send rudder angle
-                    data = posr();
-                    myFloat.m_float = data;
-                    data = myFloat.m_float;   // get the float back from the union
-                    pc.printf("%.1f", data);
-                    Thread::wait(ff);*/
-                    //send latitude
-                    data = myGPS.latitude;
-                    myFloat.m_float = data;
-                    data = myFloat.m_float;   // get the float back from the union
-                    pc.printf("%.3f", data);
-                    Thread::wait(ff);
-                    //send logitude
-                    data = myGPS.longitude;
-                    myFloat.m_float = data;
-                    data = myFloat.m_float;   // get the float back from the union
-                    pc.printf("%.3f", data);
-                    Thread::wait(ff);
-                    //send yaw
-                    yaw_read();
-                    roll_read();
-                    //send transmittion
-                    pc.printf("q");
-                    Thread::wait(1000);
-                }//if (myGPS.fix)
-            }//if (refresh_Timer..
-        }//if ( myGPS.newNMEAreceived() )
-    }//while(true)
-} // telemetry_callback()
-
-void mast_callback(void)
-{
-
-    I=0; //sets for 1.5amp output
-    motor.period(.001); //sets the pulse width modulation period
-    motor.pulsewidth(0);
-    slp = 0;
-    r_slp = 0; //sstarts the current driver in off state
-    Thread::wait(10);
-    // loop
-    while(1) {
-
-        if (rx.valid) {
-            RC_O = rx.channel[0];
-        } else {
-            //pc.printf(" invalid\r\n");
-            slp = 0;
-        }
-        //39.19
-        d_ang = (RC_O/6.77)-10.0;      //(RC_O/6.77)+39.19;
-        pos = posi();
-        //printf("%f\n",ain);
-        if((pos > (d_ang-gg))&& (pos < (d_ang+gg))) {
-            motor.pulsewidth(0);
-            slp = 0;
-        }
-        if( (pos > (d_ang+gg))) {
-            slp = 1;
-            dir = 1; //left??
-            motor.pulsewidth(.0005);
-            //pc.printf("pos: %.3f\n",pos);
-            Thread::wait(ww);
-            pos = posi();
-        }//if pos
-        if((pos < (d_ang-gg))) {
-            slp = 1;
-            dir = 0; //right??
-            motor.pulsewidth(.0005);
-            //pc.printf("pos: %.3f\n",pos);
-            Thread::wait(ww);
-            pos = posi();
-        }
-        ThisThread::sleep_until(zz);
-    }//while(1)
-}//mast callback
-
-void rudder_callback(void)
-{
-
-    r_I=0; //sets for 1.5amp output
-    rudder.period(.001); //sets the pulse width modulation period
-    rudder.pulsewidth(0);
-    slp = 0;
-    r_slp = 0; //sstarts the current driver in off state
-    Thread::wait(10);
-
-    // loop
-    while(1) {
-
-        if (rx.valid) {
-            RC_1 = rx.channel[1];
-        } else {
-            //pc.printf(" invalid\r\n");
-            r_slp = 0;
-        }
-        //39.19
-        r_ang = (RC_1/6.77)-5.0;
-        r_pos = posr();
-
-        if((r_pos > (r_ang-xx)) && (r_pos < (r_ang+xx))) {
-            rudder.pulsewidth(0);
-            r_slp = 0;
-        }
-        if( (r_pos > (r_ang+xx)) ) {  //&& r_pos < 235.0
-            r_slp = 1;
-            r_dir = 1; //left??
-            rudder.pulsewidth(.0005);
-            Thread::wait(ww);
-            r_pos = posr();
-        }//if pos
-        if((r_pos < (r_ang-xx)) ) {   // && r_pos > 55.0
-            r_slp = 1;
-            r_dir = 0; //right??
-            rudder.pulsewidth(.0005);
-            Thread::wait(ww);
-            r_pos = posr();
-        }
-        ThisThread::sleep_until(zz);
-    }//while(1)
-}//rudder callback
\ No newline at end of file
+}
\ No newline at end of file