3/18/19
Dependencies: mbed BNO055 MBed_Adafruit-GPS-Library
Revision 1:87f1199672a1, committed 2019-03-18
- Comitter:
- LukeMar
- Date:
- Mon Mar 18 04:06:52 2019 +0000
- Parent:
- 0:f4edd3407cc5
- Commit message:
- Stepper CAN node
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r f4edd3407cc5 -r 87f1199672a1 main.cpp --- a/main.cpp Thu Jan 17 00:28:07 2019 +0000 +++ b/main.cpp Mon Mar 18 04:06:52 2019 +0000 @@ -1,340 +1,75 @@ - -/* - Sailbot Hull 14 - L Marino and D Robinson, - with D Evangelista, P Frontera, M Kutzer, A Laun - 2018 - - Threaded version using mbed OS5 - Serial links between nodes - Stepper motors with Pololu 2968 drivers (MP6500 based) - Piher PST360 angle sensors - Adafruit Absolute GPS - Telemetry via ???Ubiquity??? -*/ - -//Thread for logging time and Gps, heading and encoder values is mostly complete -//control thread is a work in progress +// Program to test the CAN bus using pins 29 and 30 on the mbed connected +// to an MCP2551 CAN transceiver bus IC +// Note that this program will only continue to transmit while the TX message is <= 8 bytes long #include "mbed.h" -#include "MBed_Adafruit_GPS.h" -#include "BNO055.h" -#include "Spektrum.h" -#include "unity.h" + +#define RUDDER_ID 1 //Address of this CAN device +#define BNO_ID 2 //Address of Control CAN device +#define TELE_ID 3 +#define RC_ID 4 + +Serial pc(USBTX, USBRX); //tx, and rx for tera term +DigitalOut led1(LED1); //heartbeat +DigitalOut led2(LED2); //CAN read activity +DigitalOut led3(LED3); //CAN write activity -Serial pc(USBTX, USBRX, 115200); -Spektrum rx(p13, p14); -int i; -int j; -float now; -//***Mast universal variables*** -AnalogIn ain(p16); -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; +CAN can(p30,p29); //CAN interface + + +Ticker pulse; + +float BNOangle; +float RC_out; +float RCval; +char datastr[20]; +CANMessage msg_read; -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 -//****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 +float CanValue(){ + strcpy(datastr, (char*)msg_read.data+1); + float value = strtod(datastr,NULL); + return value; +} + +void alive(void){ + led1 = !led1; + if(led1) + pulse.attach(&alive, .2); // the address of the function to be attached (flip) and the interval (2 seconds) + else + pulse.attach(&alive, 1.3); // the address of the function to be attached (flip) and the interval (2 seconds) +} -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(35); +float RC_Read(){ + + while(can.read(msg_read)){ //if message is available, read into msg + if(msg_read.id == RC_ID){ //if its the BNO ID + if(msg_read.data[0] == 'r' || msg_read.data[0] == 'R'){ + led3 = !led3; + RC_out = CanValue(); + return RC_out; + }//if the first letter was an 's', it is a servo command + } //was the message to this device? + }//while a CAN message has been read + //return datastr; } -//********concatenate time (not currently in use)********** -float conc(float hr, float min, float sec ) -{ - return hr+(min/60.0)+(sec/3600.0); -} -//********Initialize IMU************ - -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); - } -} - -// *****threading***** -Thread telemetry_thread; -Thread mast_thread; -Thread rudder_thread; - -void telemetry_callback(void); -void mast_callback(void); -void rudder_callback(void); - -int main() -{ - //start all threads - wait(10); - //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 -}//end main - -float posi() -{ - float p1; - float p2; - float p3; - p1 = (ain-.108)/.002466; - Thread::wait(3); - p2 = (ain-.108)/.002466; - Thread::wait(3); - p3 = (ain-.108)/.002466; - printf("%f\n",ain); - 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; -} - -void telemetry_callback(void) -{ - //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 - - 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(1000); - 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(); - - 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 - 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(35); - //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); - //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); - //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); - //send yaw - yaw_read(); - //send transmittion - pc.printf("q"); - Thread::wait(3000); - }//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) { +int main() { + + Thread::wait(200); + + pulse.attach(&alive, 2.0); // the address of the function to be attached (alive) and the interval (2 seconds) + can.frequency(500000); + pc.baud(115200); + + pc.printf("%s\r\n", __FILE__); + + while(1){ + + RCval = RC_Read(); + pc.printf("%.1f\n",RCval); + } - if (rx.valid) { - RC_O = rx.channel[0]; - } else { - //pc.printf(" invalid\r\n"); - 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))) { - motor.pulsewidth(0); - slp = 0; - } - if( (pos > (d_ang+xx))) { - 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-xx))) { - 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 = (r_ain-.108)/.002466; - - 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 + +}//main \ No newline at end of file