Matthew Marino
JAN 16 2019
Dependencies: mbed BNO055 MBed_Adafruit-GPS-Library
Diff: main.cpp
- Revision:
- 1:4b4d5d18fc57
- Parent:
- 0:f4edd3407cc5
--- a/main.cpp Thu Jan 17 00:28:07 2019 +0000
+++ b/main.cpp Thu Mar 14 22:09:48 2019 +0000
@@ -28,7 +28,7 @@
int j;
float now;
//***Mast universal variables***
-AnalogIn ain(p16);
+AnalogIn ain(p20);
PwmOut motor( p25 );
DigitalOut dir( p24 );
DigitalOut I(p26);
@@ -46,9 +46,11 @@
float r_pos;
float RC_1;
-float xx = 4.0; //changes the threshold that the motor goes to sleep on
-float zz = 88; //changes the wait time at end of the the mast rudder threads
-float ww = 10; //changes wait time at end of if statments in mast rudder threads
+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;
@@ -57,6 +59,8 @@
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
+
union Float { //slightly mysterious data type that we use to send data over serial
float m_float;
@@ -78,7 +82,18 @@
myFloat.m_float = data;
data = myFloat.m_float; // get the float back from the union
pc.printf("%.1f", data);
- Thread::wait(35);
+ Thread::wait(ff);
+
+}
+
+void roll_read()
+{
+ 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);
}
@@ -119,8 +134,8 @@
int main()
{
//start all threads
- wait(10);
- //telemetry_thread.start(callback(telemetry_callback));
+ wait(1);
+ telemetry_thread.start(callback(telemetry_callback));
mast_thread.start(callback(mast_callback));
rudder_thread.start(callback(rudder_callback));
//could have a heart beat but I tried to reduce the number of threads fighting for time
@@ -131,12 +146,12 @@
float p1;
float p2;
float p3;
- p1 = (ain-.108)/.002466;
+ p1 = ain*449.0-147.53; //ain*447.73-147.53;
Thread::wait(3);
- p2 = (ain-.108)/.002466;
+ p2 = ain*447.73-147.53;
Thread::wait(3);
- p3 = (ain-.108)/.002466;
- printf("%f\n",ain);
+ p3 = ain*447.73-147.53;
+
return (p1+p2+p3)/3.0;
}
float posr()
@@ -157,7 +172,7 @@
//uint64_t now;
//debug("telemetry_thread started\r\n");
- pc.baud(115200); //sets virtual COM serial communication to high rate; this is to allow more time to be spent on GPS retrieval
+
gps_Serial = new Serial(p9,p10); //serial object for use w/ GPS
Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
@@ -171,7 +186,7 @@
//pc.printf("Connection established at 115200 baud...\n");
bno_init();
- Thread::wait(1000);
+ Thread::wait(100);
refresh_Timer.start(); //starts the clock on the timer
while(true) {
@@ -186,13 +201,13 @@
if (refresh_Timer.read_ms() >= refresh_Time) {
refresh_Timer.reset();
-
- data = conc(myGPS.hour, myGPS.minute, myGPS.seconds ); // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
+
+ /* 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(35);
- /* this part can send hr min second separately, commented to save thread space
+ 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
@@ -210,38 +225,39 @@
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();
+ /* data = posi();
myFloat.m_float = data;
data = myFloat.m_float; // get the float back from the union
pc.printf("%.1f", data);
- Thread::wait(35);
+ 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(35);
+ 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("%.2f", data);
- Thread::wait(35);
+ 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("%.2f", data);
- Thread::wait(35);
+ pc.printf("%.3f", data);
+ Thread::wait(ff);
//send yaw
yaw_read();
+ roll_read();
//send transmittion
pc.printf("q");
- Thread::wait(3000);
+ Thread::wait(1000);
}//if (myGPS.fix)
}//if (refresh_Timer..
}//if ( myGPS.newNMEAreceived() )
@@ -253,13 +269,13 @@
I=0; //sets for 1.5amp output
motor.period(.001); //sets the pulse width modulation period
- motor.pulsewidth(0);
+ 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 {
@@ -267,28 +283,28 @@
slp = 0;
}
//39.19
- d_ang = (RC_O/6.77)+39.19;
- pos = (ain-.108)/.002466;
- printf("%f\n",ain);
- if((pos > (d_ang-xx))&& (pos < (d_ang+xx))) {
+ 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+xx))) {
+ if( (pos > (d_ang+gg))) {
slp = 1;
- dir = 1; //left??
- motor.pulsewidth(.0005);
- //pc.printf("pos: %.3f\n",pos);
- Thread::wait(ww);
- pos = posi();
+ dir = 1; //left??
+ motor.pulsewidth(.0005);
+ //pc.printf("pos: %.3f\n",pos);
+ Thread::wait(ww);
+ pos = posi();
}//if pos
- if((pos < (d_ang-xx))) {
+ if((pos < (d_ang-gg))) {
slp = 1;
- dir = 0; //right??
- motor.pulsewidth(.0005);
- //pc.printf("pos: %.3f\n",pos);
- Thread::wait(ww);
- pos = posi();
+ dir = 0; //right??
+ motor.pulsewidth(.0005);
+ //pc.printf("pos: %.3f\n",pos);
+ Thread::wait(ww);
+ pos = posi();
}
ThisThread::sleep_until(zz);
}//while(1)
@@ -299,14 +315,14 @@
r_I=0; //sets for 1.5amp output
rudder.period(.001); //sets the pulse width modulation period
- rudder.pulsewidth(0);
+ 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 {
@@ -315,7 +331,7 @@
}
//39.19
r_ang = (RC_1/6.77)-5.0;
- r_pos = (r_ain-.108)/.002466;
+ r_pos = posr();
if((r_pos > (r_ang-xx)) && (r_pos < (r_ang+xx))) {
rudder.pulsewidth(0);
@@ -323,17 +339,17 @@
}
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();
+ 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();
+ r_dir = 0; //right??
+ rudder.pulsewidth(.0005);
+ Thread::wait(ww);
+ r_pos = posr();
}
ThisThread::sleep_until(zz);
}//while(1)