STEM ELF
/
mbed_daugher_v0_ranging
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Servo USBDevice mbed
Fork of
USBSerial_HelloWorld
by 
Samuel Mokrani
Revision 10:d74124f266cb, committed 2016-08-10
- Comitter:
- matthewgascoyne
- Date:
- Wed Aug 10 08:01:00 2016 +0000
- Parent:
- 9:d88699a0905a
- Commit message:
- base version of OpenSTEM Ranging Board Firmware
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Wed Aug 10 08:01:00 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/command_matrix.h Wed Aug 10 08:01:00 2016 +0000
@@ -0,0 +1,39 @@
+// This file firstly defines a lookup table of all the valid
+// command IDs and the number of data bytes that each should have.
+// Below the lookup table is a memory used to store those data
+// bytes when a valid command is received.
+#include "stdint.h"
+
+/////////////////////LOOKUP TABLE////////////////////////////////////////
+extern const uint8_t c_matrix[26]= {
+ 'f' , 3 , // ID = "f"(0x66) , 3 data bytes
+ 's' , 2 , // ID = "s"(0x73) , 2 data bytes
+ 'i' , 0 , // ID = "i"(0x69) , 0 data bytes
+ 'y' , 0 , // ID = "y"(0x79) , 0 data bytes
+ 'p' , 0 , // ID = "p"(0x70) , 0 data bytes
+ 'm' , 2 , // ID = "m"(0x6D) , 2 data bytes
+ 'd' , 0 , // ID = "d"(0x64) , 0 data bytes
+ 'r' , 3 , // ID = "r"(0x72) , 3 data bytes
+ 'q' , 3 , // ID = "q"(0x71) , 3 data bytes
+ 't' , 0 , // ID = "t"(0x74) , 0 data bytes
+ 'v' , 0 , // ID = "v"(0x76) , 0 data bytes
+ 'o' , 1 , // ID = "o"(0x6F) , 1 data bytes
+ 'u' , 0 , // ID = "u"(0x75) , 0 data bytes
+ };
+/////////////////////////////////////////////////////////////////////////
+
+///////////////////DATA BYTE STORAGE/////////////////////////////////////
+extern uint8_t c_data_0x66[3]= { 0, 0, 0, }; //data holder for "f" 0x66
+extern uint8_t c_data_0x73[2]= { 0, 0, }; //data holder for "s" 0x73
+ //do not need a data holder for "i" 0x69
+ //do not need a data holder for "y" 0x79
+ //do not need a data holder for "p" 0x70
+extern uint8_t c_data_0x6D[2]= { 0, 0, }; //data holder for "m" 0x6D
+ //do not need a data holder for "d" 0x64
+extern uint8_t c_data_0x72[3]= { 0, 0, 0, }; //data holder for "r" 0x72
+extern uint8_t c_data_0x71[3]= { 0, 0, 0, }; //data holder for "q" 0x71
+ //do not need a data holder for "t" 0x74
+ //do not need a data holder for "v" 0x76
+extern uint8_t c_data_0x6F[1]= { 0, }; //data holder for "o" 0x6F
+ //do not need a data holder for "u" 0x75
+/////////////////////////////////////////////////////////////////////////
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/core_defines.h Wed Aug 10 08:01:00 2016 +0000 @@ -0,0 +1,70 @@ +/////// FIRMWARE HEADER DETAILS +#define _FW_VERSION 0.0 +#define _HW_VERSION "A" +#define _UNIQUE_NAME "STEM001" +#define _DAUGHTER_NAME "DAU_000" +#define _PARENT_NAME "RANGING" +#define _BOOT_MESSAGE "MBED DAUGHTER BOARD FIRMWARE\r\n" + +////////////////////////////////// PIN DIRECTION +#define _disc0_Dir 0 // 1=output, 0=input +#define _disc1_Dir 0 // 1=output, 0=input +#define _disc2_Dir 1 // 1=output, 0=input +#define _sync_Dir 0 // 1=output, 0=input +#define _directsync_Dir 0 // output not allowed, 0=input +#define _fdetect_Dir 0 // output not allowed, 0=input +#define _p29_IO_Dir 1 // 1=output, 0=input +#define _p30_IO_Dir 1 // 1=output, 0=input + +////////////////////////////////// PIN FUNCTION +#define _disc0_AD 1 // 1=analogue, 0=digital +#define _disc1_AD 1 // 1=analogue, 0=digital +#define _disc2_AD 0 // 1=analogue, 0=digital +#define _sync_AD 1 // 1=analogue, 0=digital +#define _directsync_AD 1 // 1=analogue, 0=digital +#define _fdetect_AD 0 // 1=analogue, 0=digital +#define _p29_IO_AD 0 // analogue unavailable, 0=digital +#define _p30_IO_AD 0 // analogue unavailable, 0=digital + +/////// PIN DEFAULT VALUE +#define _def_disc0 0 +#define _def_disc1 0 +#define _def_disc2 0 +#define _def_sync 0 +#define _def_p29_out 0 +#define _def_p30_out 0 +#define _def_sdigioutA 0 +#define _def_sdigioutB 0 +#define _def_sdigioutC 0 +#define _def_sdigioutD 0 +#define _def_sdigioutE 0 +#define _def_sdigioutF 0 + +/////// SERVO DEFAULT VALUE +#define _def_servo1 0.0 +#define _def_servo2 0.0 +#define _def_servo3 0.5 + +/////// DEFAULT MUX CONFIG +#define _def_mux00_ch 3 +#define _def_mux01_ch 2 +#define _def_mux02_ch 3 +#define _def_mux03_ch 1 +#define _def_mux04_ch 2 + +/////// DEFAULT SERIAL CONFIG +#define _USB_BAUDRATE 57600 +#define _USB_RECEIVE_BUF 255 +#define _USB_TX_WAIT_US 10 + +/////// MESSAGE DELIMITERS +#define _HOST_MSG_START 0x23 // "#" character +#define _MBED_MSG_START 0x7E // "~" character +#define _MSG_END 0x5C // "\" character + +/////// FOURIER PARAMETERS +#define _RPM_CHANGE_DELAY_MS 85 + +/////// RANGING PARAMETERS +#define _ROTATE_SPEED 0.1 +#define _ROTATE_TIME_MS 75
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/daughter_core.cpp Wed Aug 10 08:01:00 2016 +0000
@@ -0,0 +1,1000 @@
+// This include file is for the core firmware functions
+// associated with the mbed daughter board.
+
+#include "core_defines.h"
+#include "command_matrix.h"
+#include "stdint.h"
+#include "Servo.h" //to include the mBed Servo library
+#include "mbed.h"
+
+extern DigitalOut shift_data(p26); // These pins are used exclusively by
+extern DigitalOut shift_srclk(p25); // the "dshift_out" function.
+extern DigitalOut shift_rclk(p24); //
+
+extern char unique_nm[8] = {0,0,0,0,0,0,0,0}; //if user sets unique name
+extern char daughter_nm[8] = {0,0,0,0,0,0,0,0}; //if user sets daughter name
+extern char parent_nm[8] = {0,0,0,0,0,0,0,0}; //if user sets parent name
+
+extern "C" void mbed_reset(); // used to remotely reset the mbed
+Timer direct_s_tim; // Set up timer for direct sync detection
+Timer range_tim; // Set up timer for ultrasonic ranging
+Ticker scan; // To make sure no dormant commands in buffer
+DigitalOut led1(LED1); // set up the heartbeat LED1
+
+#if _p29_IO_Dir
+DigitalOut p29_out(p29);
+#else
+DigitalIn p29_in(p29);
+#endif
+
+#if _p30_IO_Dir
+DigitalOut p30_out(p30);
+#else
+DigitalIn p30_in(p30);
+#endif
+
+#if _disc0_Dir //if configured as output
+DigitalOut disc0_out(p15);
+#else //if configured as input
+#if _disc0_AD //if input & analogue
+AnalogIn disc0_in(p15);
+#else //otherwise input & digital
+DigitalIn disc0_in(p15);
+#endif
+#endif
+
+#if _disc1_Dir //if configured as output
+DigitalOut disc1_out(p16);
+#else //if configured as input
+#if _disc1_AD //if input & analogue
+AnalogIn disc1_in(p16);
+#else //otherwise input & digital
+DigitalIn disc1_in(p16);
+#endif
+#endif
+
+#if _disc2_Dir //if configured as output
+DigitalOut disc2_out(p17);
+#else //if configured as input
+#if _disc2_AD //if input & analogue
+AnalogIn disc2_in(p17);
+#else //otherwise input & digital
+DigitalIn disc2_in(p17);
+#endif
+#endif
+
+#if _sync_Dir //if configured as output
+#if _sync_AD //if output & analogue
+AnalogOut sync_out(p18);
+#else //if output & digital
+DigitalOut sync_out(p18);
+#endif
+#else //if configured as input
+#if _sync_AD //if input and analogue
+AnalogIn sync_in(p18);
+#else //if input and digital
+DigitalIn sync_in(p18);
+#endif
+#endif
+
+#if _directsync_AD //if input & analogue
+AnalogIn directsync_in(p19);
+#else //if input and digital
+DigitalIn directsync_in(p19);
+#endif
+
+#if _fdetect_AD //if input and analogue
+AnalogIn fdetect_in(p20);
+#else //if input and digital
+DigitalIn fdetect_in(p20);
+#endif
+
+Servo s_ervo1(p23);
+Servo s_ervo2(p22);
+Servo s_ervo3(p21);
+
+extern uint8_t mux00_ch = _def_mux00_ch;//channel select for Mux00 (0,1,2 or 3)
+extern uint8_t mux01_ch = _def_mux01_ch;//channel select for Mux01 (0,1,2 or 3)
+extern uint8_t mux02_ch = _def_mux02_ch;//channel select for Mux02 (0,1,2 or 3)
+extern uint8_t mux03_ch = _def_mux03_ch;//channel select for Mux03 (0,1,2 or 3)
+extern uint8_t mux04_ch = _def_mux04_ch;//channel select for Mux04 (0,1,2 or 3)
+
+extern bool sdigi_a = _def_sdigioutA; //shift digital channel output a (0 or 1)
+extern bool sdigi_b = _def_sdigioutB; //shift digital channel output b (0 or 1)
+extern bool sdigi_c = _def_sdigioutC; //shift digital channel output c (0 or 1)
+extern bool sdigi_d = _def_sdigioutD; //shift digital channel output d (0 or 1)
+extern bool sdigi_e = _def_sdigioutE; //shift digital channel output e (0 or 1)
+extern bool sdigi_f = _def_sdigioutF; //shift digital channel output f (0 or 1)
+
+extern Serial pc(USBTX, USBRX); //the USB serial channel
+uint8_t usb_receive_buf[_USB_RECEIVE_BUF]; //setup serial receive buffer
+extern uint8_t usb_receive_pnt = 0; //pointer to position in buffer
+extern bool usb_receive_flg = false; //true=new byte, false=NO new byte
+
+/////////////////////////////////////////////////////////
+// "print_array" : Function to send all character bytes
+// from an array out through the serial port.
+void print_array(char *array, uint8_t size){
+ for (uint8_t n = 0; n < size; n++){
+ while((!pc.writeable())){wait_us(_USB_TX_WAIT_US);}
+ if(array[n] == '\0'){ break; }
+ pc.putc(array[n]);
+ }
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "boot_msg" : Function to send a bootup message via the
+// usb serial link.
+void boot_msg(void){
+ pc.printf("F/W VERSION:%3.1f\r\n",_FW_VERSION);
+ pc.printf("H/W VERSION:");
+ pc.printf(_HW_VERSION);
+ pc.printf("\r\n");
+ pc.printf(" UNIQUE:");
+ if(unique_nm[0] != 0){
+ print_array(unique_nm, sizeof(unique_nm)); }
+ else { pc.printf(_UNIQUE_NAME); }
+ pc.printf("\r\n");
+ pc.printf(" DAUGHTER:");
+ if(daughter_nm[0] != 0){
+ print_array(daughter_nm, sizeof(daughter_nm)); }
+ else { pc.printf(_DAUGHTER_NAME); }
+ pc.printf("\r\n");
+ pc.printf(" PARENT:");
+ if(parent_nm[0] != 0){
+ print_array(parent_nm, sizeof(parent_nm)); }
+ else { pc.printf(_PARENT_NAME); }
+ pc.printf("\r\n");
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "command_clean" : Function to clean a command from the
+// recieve buffer quickly.
+void command_clean(uint8_t t_pnt, uint8_t c_pnt){
+ //remove command from buffer
+ for(uint8_t m=t_pnt; m<=c_pnt; m++){
+ usb_receive_buf[m] = 0x00;
+ }
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "diag_reply" : Function to send diagnostics info back
+// to the host PC via the USB serial.
+void diag_reply(void){
+ char stream[21] = {
+ _MBED_MSG_START, 'd',
+ 'M', (mux00_ch+48) , (mux01_ch+48) , (mux02_ch+48) ,
+ (mux03_ch+48) , (mux04_ch+48) ,
+ 'S', (sdigi_a+48) , (sdigi_b+48) , (sdigi_c+48) ,
+ (sdigi_d+48) , (sdigi_e+48) , (sdigi_f+48) ,
+ 'F', (p29_out+48) , (p30_out+48) ,
+ _MSG_END , 0x00 , '\0' ,};
+ uint8_t size = ((sizeof(stream))-2);
+ for(uint8_t n=0; n<size; n++){
+ stream[size] = stream[size] ^ stream[n];
+ }
+ if(stream[size] == _HOST_MSG_START){stream[size]++;}
+ if(stream[size] == _MBED_MSG_START){stream[size]++;}
+ if(stream[size] == _MSG_END){stream[size]++;}
+ print_array(stream, sizeof(stream));
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "dshift_out" : Function to shift data out via the
+// shift register.
+// This will effect the digital shift outputs and the
+// multiplexer configuration.
+void dshift_out(uint16_t sdata_out) {
+ shift_data = 0; shift_srclk = 0; shift_rclk = 0;
+ wait(0.005);
+ for(char n=0; n<16; n++)
+ {
+ if(1<<n & sdata_out)
+ {
+ shift_data = 1;
+ }
+ else
+ {
+ shift_data = 0;
+ }
+ wait(0.0001);
+ shift_srclk = 1;
+ wait(0.0001);
+ shift_srclk = 0;
+ }
+ wait(0.0001);
+ shift_rclk = 1;
+ wait(0.0001);
+ shift_rclk = 0;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "set_servo" : Function sets the servo speed as a
+// floating point fraction of 100%.
+void set_servo(uint8_t servo, float speedval){
+ switch(servo){
+ case 1: s_ervo1 = speedval; break;
+ case 2: s_ervo2 = speedval; break;
+ case 3: s_ervo3 = speedval; break;
+ }
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "freq_reply" : Function send a reply message back to
+// the host PC to say which frequency was detected on
+// the directsync pin. This will have been applied to the
+// motordrive also.
+void freq_reply(int freq, uint8_t command){
+ char stream[8] = {_MBED_MSG_START , command ,
+ '0' , '0' , '0' , _MSG_END , 0 , '\0' ,};
+ stream[2] = (freq/100);
+ stream[3] = (freq - (stream[2]*100))/10;
+ stream[4] = (freq - ((stream[2]*100)+(stream[3]*10)));
+ stream[2] = stream[2] + 48;
+ stream[3] = stream[3] + 48;
+ stream[4] = stream[4] + 48;
+ uint8_t size = ((sizeof(stream))-2);
+ for(uint8_t n=0; n<size; n++){
+ stream[size] = stream[size] ^ stream[n];
+ }
+ if(stream[size] == _HOST_MSG_START){stream[size]++;}
+ if(stream[size] == _MBED_MSG_START){stream[size]++;}
+ if(stream[size] == _MSG_END){stream[size]++;}
+ print_array(stream, sizeof(stream));
+ //pc.printf("%d\r\n",freq);
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "volts_IR" : Function read the analogue input voltage
+// present on the directsync_in pin
+void volts_IR(void){
+ float the_volts = directsync_in;
+ the_volts = the_volts * 3.3f * 100;
+ int rounder = the_volts;
+ float fraction = the_volts - rounder;
+ //pc.printf(" %5.3f ", the_volts);
+ //pc.printf(" %5.3f ", fraction);
+ //pc.printf(" %d ", rounder);
+ if(fraction >= 0.5f){ the_volts += 1; }
+ char stream[8] = {_MBED_MSG_START , 'v' ,
+ '0' , '0' , '0' , _MSG_END , 0 , '\0' ,};
+ stream[2] = (the_volts / 100);
+ stream[3] = (the_volts - (stream[2]*100)) / 10;
+ stream[4] = the_volts - (stream[2]*100) - (stream[3]*10);
+ stream[2] = stream[2] + 48;
+ stream[3] = stream[3] + 48;
+ stream[4] = stream[4] + 48;
+ uint8_t size = ((sizeof(stream))-2);
+ for(uint8_t n=0; n<size; n++){
+ stream[size] = stream[size] ^ stream[n];
+ }
+ if(stream[size] == _HOST_MSG_START){stream[size]++;}
+ if(stream[size] == _MBED_MSG_START){stream[size]++;}
+ if(stream[size] == _MSG_END){stream[size]++;}
+ print_array(stream, sizeof(stream));
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "directsync" : Function read the input frequency on
+// the direct sync pin. This frequency should then be
+// applied to the motor drive. This routine will then
+// return the frequency it applied.
+int directsync(void){
+ uint8_t fstate = 0;
+ int freq = 0;
+ uint16_t n;
+ int begin = 0;
+ int end = 1;
+ for(n = 0; n<1000; n++){ //wait up to 250ms for
+ if(directsync_in == 0){ //pin to be 0 (low)
+ fstate = 1; break; }
+ wait_us(250);
+ }
+ if(fstate == 0){ return 0;} //never went low so quit
+ else{
+ for(n = 0; n<2000; n++){ //now wait 100ms for pin
+ if(directsync_in == 1){ //to be 1 (high)
+ direct_s_tim.start();
+ fstate = 2;
+ begin = direct_s_tim.read_us(); break;
+ } //if high, start timer
+ wait_us(50);
+ }
+ }
+ if(fstate != 2){ return 0; }//no high seen, then quit
+ else{ //we saw a high so wait up
+ for(n = 0; n<2000; n++){ //to 100ms for a (low)
+ if(directsync_in == 0){
+ fstate = 3; break;//sen low, so carry on
+ }
+ wait_us(50);
+ }
+ }
+ if(fstate != 3){ return 0; } //no low see, so quit
+ else{ //low seen so now we
+ for(n = 0; n<2000; n++){ //wait up to 100ms for
+ if(directsync_in == 1){ //the final high
+ fstate = 4;
+ end = direct_s_tim.read_us(); break;
+ } //high seen, stop timer
+ wait_us(50);
+ }
+ }
+ direct_s_tim.stop();
+ freq = 1000000 / (end - begin);
+ return freq;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "get_disc_freq" : Function read the frequency of the
+// spinning disc. This routine will then
+// return the frequency measured.
+int get_disc_freq(void){
+ uint8_t fstate = 0;
+ int freq = 0;
+ uint16_t n;
+ int begin = 0;
+ int end = 1;
+ for(n = 0; n<1000; n++){ //wait up to 250ms for
+ if(fdetect_in == 0){ //pin to be 0 (low)
+ fstate = 1; break; }
+ wait_us(250);
+ }
+ if(fstate == 0){ return 0;} //never went low so quit
+ else{
+ for(n = 0; n<2000; n++){ //now wait 100ms for pin
+ if(fdetect_in == 1){ //to be 1 (high)
+ direct_s_tim.start();
+ fstate = 2;
+ begin = direct_s_tim.read_us(); break;
+ } //if high, start timer
+ wait_us(50);
+ }
+ }
+ if(fstate != 2){ return 0; }//no high seen, then quit
+ else{ //we saw a high so wait up
+ for(n = 0; n<2000; n++){ //to 100ms for a (low)
+ if(fdetect_in == 0){
+ fstate = 3; break;//sen low, so carry on
+ }
+ wait_us(50);
+ }
+ }
+ if(fstate != 3){ return 0; } //no low see, so quit
+ else{ //low seen so now we
+ for(n = 0; n<2000; n++){ //wait up to 100ms for
+ if(fdetect_in == 1){ //the final high
+ fstate = 4;
+ end = direct_s_tim.read_us(); break;
+ } //high seen, stop timer
+ wait_us(50);
+ }
+ }
+ direct_s_tim.stop();
+ freq = 1000000 / (end - begin);
+ return freq;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "ultra_range" : Function performs an ultrasonic range
+// measurement and outputs the duration of the sensor
+// output pulse.
+void ultra_range(void){
+ uint8_t rstate = 0;
+ int begin = 0;
+ int end = 1;
+ float range = 0;
+ p29_out = 0; //send the 50us
+ p29_out = 1; //pulse to trigger
+ wait_us(50); //the ultrasonic
+ p29_out = 0; //
+ range_tim.start(); //start the timer
+ for(int n=0; n<1000; n++){
+ if(fdetect_in == 1){
+ begin = range_tim.read_us();
+ rstate = 1;
+ //pc.printf(" %d begin,\r\n",begin);
+ break;
+ }
+ else {wait_us(1);}
+ }
+ if(rstate != 1){}
+ else{
+ for(uint16_t m=0; m<31000; m++){
+ if(fdetect_in == 0){
+ end = range_tim.read_us();
+ rstate = 2;
+ //pc.printf(" %d end.\r\n",end);
+ break;
+ }
+ else {wait_us(1);}
+ }
+ }
+ if(rstate != 2){}
+ else{
+ range_tim.stop();
+ range_tim.reset();
+ range = end - begin;
+ range = (range / 5.80);
+ //pc.printf(" %5.1fmm range.\r\n",range);
+ }
+ if(range > 999.0){ range = 999.0; }
+ char stream[8] = {_MBED_MSG_START , 'u' ,
+ '0' , '0' , '0' , _MSG_END , 0 , '\0' ,};
+ stream[2] = (range / 100);
+ stream[3] = (range - (stream[2]*100)) / 10;
+ stream[4] = range - (stream[2]*100) - (stream[3]*10);
+ stream[2] = stream[2] + 48;
+ stream[3] = stream[3] + 48;
+ stream[4] = stream[4] + 48;
+ uint8_t size = ((sizeof(stream))-2);
+ for(uint8_t n=0; n<size; n++){
+ stream[size] = stream[size] ^ stream[n];
+ }
+ if(stream[size] == _HOST_MSG_START){stream[size]++;}
+ if(stream[size] == _MBED_MSG_START){stream[size]++;}
+ if(stream[size] == _MSG_END){stream[size]++;}
+ print_array(stream, sizeof(stream));
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "set_discfreq" : Function attempts to set the disc
+// rpm to the defined value. When rpm is reached it stops
+// trying.
+void set_discfreq(int discrpm){
+ uint8_t count = 0;
+ //float gain = 0.001;
+ float newgain = 0.001;
+ for(uint8_t n = 0; n<250; n++){
+ int reading = get_disc_freq();
+ newgain = (discrpm - reading);
+ if(newgain > 40){ newgain = 40; }
+ if(newgain < -40){ newgain = -40; }
+ newgain = (newgain / 10000);
+ s_ervo1 = s_ervo1 + newgain;
+ //if(reading < discrpm){ s_ervo1 = s_ervo1 + gain; }
+ //if(reading > discrpm){ s_ervo1 = s_ervo1 - gain; }
+ if(reading == discrpm){ count++;
+ if(count >= 10){ break; }
+ }
+ wait_ms(_RPM_CHANGE_DELAY_MS);
+ }
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "dshift_combine" : Function to combine all shift reg
+// elements into a single 16bit variable, for shifting.
+// This will return the 16bit variable.
+uint16_t dshift_combine(void) {
+ uint16_t sdata_out = 0x0000; //clear the output shifter
+ sdata_out |= (sdigi_a << 15); //now combine all elements
+ sdata_out |= (sdigi_b << 14); //into the 16bits
+ sdata_out |= (sdigi_c << 13); //
+ sdata_out |= (sdigi_d << 12); //
+ sdata_out |= (sdigi_e << 11); //
+ sdata_out |= (sdigi_f << 10); //
+ sdata_out |= (mux00_ch << 8); //
+ sdata_out |= (mux01_ch << 6); //
+ sdata_out |= (mux02_ch << 4); //
+ sdata_out |= (mux03_ch << 2); //
+ sdata_out |= mux04_ch; //
+ return sdata_out;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "set_mux_chan" : Function to configure one of the five
+// daughter board multiplexers. (0 to 4). Each multiplexer
+// can select channels 1,2,3 or high-impedance (0).
+// This will effect the digital shift outputs and the
+// multiplexer configuration.
+void set_mux_chan(uint8_t mux, uint8_t chan) {
+ switch(mux) {
+ case 0: mux00_ch = chan; break;
+ case 1: mux01_ch = chan; break;
+ case 2: mux02_ch = chan; break;
+ case 3: mux03_ch = chan; break;
+ case 4: mux04_ch = chan; break;
+ }
+ uint16_t sdata_out = 0x0000; //clear the output shifter
+ sdata_out = dshift_combine(); //combine into 16bits
+ dshift_out(sdata_out); //perform the shift
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "fdigi_out" : Function to assert digital outputs
+// attached directly to the mbed pins p29 and p30.
+void fdigi_out(uint8_t pin, bool pinval){
+ switch(pin) {
+ case 29: if(_p29_IO_Dir){p29_out = pinval;} break;
+ case 30: if(_p30_IO_Dir){p30_out = pinval;} break;
+ }
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "sdigi_out" : Function to assert digital outputs
+// attached to the daughter board shift register.
+// digi channels a, b, c, d, e and f.
+// This will effect the digital shift outputs only.
+void sdigi_out(char digi_ch, bool pinval) {
+ switch(digi_ch) {
+ case 'a': sdigi_a = pinval; break;
+ case 'b': sdigi_b = pinval; break;
+ case 'c': sdigi_c = pinval; break;
+ case 'd': sdigi_d = pinval; break;
+ case 'e': sdigi_e = pinval; break;
+ case 'f': sdigi_f = pinval; break;
+ }
+ uint16_t sdata_out = 0x0000; //clear the output shifter
+ sdata_out = dshift_combine(); //combine into 16bits
+ dshift_out(sdata_out); //perform the shift
+}
+/////////////////////////////////////////////////////////
+
+
+/////////////////////////////////////////////////////////
+// "rotate_scene_cont" : Function to turn ranging scene
+// to the left or the right by a small amount
+void rotate_scene_cont(uint8_t direction){
+ while(disc1_in < 0.5f){} //wait for signal to go high
+ wait_us(100); //short pause
+ while(disc1_in > 0.5f){} //wait for signal to go low
+ set_mux_chan(0,2); //allow rotate servo to move
+ switch(direction){
+ case 'l': {s_ervo3 = _def_servo3 + _ROTATE_SPEED; break;}
+ case 'L': {s_ervo3 = _def_servo3 + _ROTATE_SPEED; break;}
+ case 'r': {s_ervo3 = _def_servo3 - _ROTATE_SPEED; break;}
+ case 'R': {s_ervo3 = _def_servo3 - _ROTATE_SPEED; break;} }
+ wait_ms(_ROTATE_TIME_MS);
+ s_ervo3 = _def_servo3;
+ while(disc1_in < 0.5f){} //wait for signal to go high
+ wait_us(100); //short pause
+ while(disc1_in > 0.5f){} //wait for signal to go low
+ set_mux_chan(0,3); //stop rotate servo from move
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "rotate_scene" : Function to turn ranging scene
+// to the left or the right by a small amount
+void rotate_scene(uint8_t direction){
+ switch(direction){
+ case 'l': {s_ervo3 = s_ervo3 + _ROTATE_SPEED; break;}
+ case 'L': {s_ervo3 = s_ervo3 + _ROTATE_SPEED; break;}
+ case 'r': {s_ervo3 = s_ervo3 - _ROTATE_SPEED; break;}
+ case 'R': {s_ervo3 = s_ervo3 - _ROTATE_SPEED; break;} }
+ if(s_ervo3 < 0.0f){ s_ervo3 = 0.0f; }
+ if(s_ervo3 > 1.0f){ s_ervo3 = 1.0f; }
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "check_usb_rec_flg" : Function to see if any bytes
+// have come in, on the USB serial. Returns the
+// usb_receive_flg.
+bool check_usb_rec_flg(void){
+ return usb_receive_flg;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "clr_usb_rec_flg" : Function to clear the
+// usb_receive_flg.
+void clr_usb_rec_flg(void){
+ usb_receive_flg = false;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "usb_serial_r" : Function to deal with serial bytes
+// being received via USB serial. When a byte is rec'd
+// it is copied to a circular buffer and then a rec'd
+// flag is asserted to show that action is needed.
+void usb_serial_r (void) {
+ //pc.printf("i'm in..\r\n");
+ uint8_t character = pc.getc();
+ if((usb_receive_pnt > (_USB_RECEIVE_BUF - 32)) &&
+ (character == _HOST_MSG_START)){
+ usb_receive_pnt = 0; //reset to start of buffer
+ usb_receive_buf[usb_receive_pnt] = character; //save
+ usb_receive_pnt++; //point to next empty buffer slot
+ }
+ else{
+ usb_receive_buf[usb_receive_pnt] = character; //save
+ usb_receive_pnt++; //point to next empty buffer slot
+ }
+ usb_receive_flg = true;
+ //pc.putc(character);
+ //pc.putc(character); pc.putc(usb_receive_buf[usb_receive_pnt-1]); //for testing
+ //pc.printf("%d\r\n",usb_receive_pnt);
+ //pc.printf("%d\r\n",usb_receive_flg);
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "usb_serial_t" : Function to send array of characters
+// to the host PC via the usb serial. Returns the number
+// of characters written to the USB serial.
+uint8_t usb_serial_t (char txstring[]){
+ uint8_t count = 0;
+ while(txstring[count] != '\0'){
+ while((!pc.writeable())){wait_us(_USB_TX_WAIT_US);}
+ pc.putc(txstring[count]);
+ count++;
+ }
+ //pc.printf("%d",count);
+ return count;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "c_lookchk" : Function to check to see if the command
+// byte exists in any of the lookup tables. If it does
+// then true is returned.
+bool c_lookchk (uint8_t command){
+ //pc.printf("command is %c\r\n",command);
+ //pc.printf("c_matrix size is %d\r\n",sizeof(c_matrix));
+ bool retval = false;
+ for(uint8_t n=0;n<sizeof(c_matrix);n+=2){
+ if(c_matrix[n] == command){
+ retval = true;
+ break;
+ }
+ }
+ return retval;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "c_looklen" : Function to lookup the defined length of
+// the given command's data bytes. Returns the number of
+// data bytes that should exist. 0xFF denotes that the
+// command wasn't found in the lookup table.
+uint8_t c_looklen (uint8_t command){
+ //pc.printf("command is %c\r\n",command);
+ //pc.printf("c_matrix size is %d\r\n",sizeof(c_matrix));
+ uint8_t retval = 0xFF;
+ for(uint8_t n=0;n<sizeof(c_matrix);n+=2){
+ if(c_matrix[n] == command){
+ retval = c_matrix[n+1];
+ break;
+ }
+ }
+ return retval;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "check_msg" : That generates the final check byte from
+// the message found between tmp_pnt and chk_pnt. If the
+// check byte matches then true is returned.
+bool check_msg(uint8_t tmp_pnt, uint8_t chk_pnt){
+
+ //usb_receive_buf[0] = 'a';
+ //usb_receive_buf[1] = 'b';
+ //usb_receive_buf[2] = 'c';
+ //usb_receive_buf[3] = '`';
+
+ bool retval = false;
+ uint8_t tempxor = 0;
+ for(uint8_t n=tmp_pnt;n<chk_pnt;n++){
+ tempxor = tempxor ^ usb_receive_buf[n];
+ //pc.printf("n is %d, tempxor is %c\r\n",n,tempxor);
+ }
+ if(tempxor == _HOST_MSG_START){ tempxor++; }
+ if(tempxor == _MSG_END) { tempxor++; }
+ if(tempxor == _MBED_MSG_START) { tempxor++; }
+ //pc.printf("tempxor is %c\r\n",tempxor);
+ //pc.printf("checkbyte is %c\r\n",usb_receive_buf[chk_pnt]);
+ if(tempxor == usb_receive_buf[chk_pnt]){
+ retval = true;
+ }
+ return retval;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "stan_reply" : Function to send a standard reply from
+// the mBed to the Host PC. A standard reply is one that
+// confirms that the message sent by the host has been
+// actioned.
+void stan_reply(char command){
+ char msg[5] = {
+ _MBED_MSG_START,
+ command,
+ _MSG_END, 0, '\0' };
+ msg[3] = (_MBED_MSG_START ^ command) ^ _MSG_END;
+ if(msg[3] == _HOST_MSG_START){ msg[3]=msg[3]+1; }
+ if(msg[3] == _MSG_END) { msg[3]=msg[3]+1; }
+ if(msg[3] == _MBED_MSG_START) { msg[3]=msg[3]+1; }
+ usb_serial_t(msg);
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "usb_serial_bufscan" : Function to check the INPUT
+// BUFFER to see if a valid command has been received. If
+// so then the function returns with the command ID.
+uint8_t usb_serial_bufscan (void){
+ bool start_flag = false;
+ uint8_t ret_cmd = 0;
+ uint8_t temp_pnt = 0; uint8_t temp_end = 0;
+ uint8_t temp_cmd = 0; uint8_t chk_pnt = 0;
+ for(uint8_t n=0;n<_USB_RECEIVE_BUF;n++){
+
+ //pc.printf("n = [%d]\r\n",n);
+
+ if(usb_receive_buf[n] == _HOST_MSG_START){
+ //pc.printf("'start' found @ [%d]\r\n",n);
+ start_flag = true; //if start found then
+ //pc.printf("start_flag = %s\r\n",start_flag ? "true" : "false");
+ temp_pnt = n; //save the start pntr.
+ //pc.printf("temp_pnt = [%d]\r\n",temp_pnt);
+ }
+ //else{ pc.printf("[%d] is not a start byte\r\n",n);}
+
+ if(((n-1) == temp_pnt) && (start_flag)){
+ //pc.printf("Previous byte was start byte\r\n");
+ //If startflag was set on previous iteration
+ //Then lookup the command (current byte)
+ start_flag = c_lookchk(usb_receive_buf[n]);
+ if(start_flag){
+ temp_cmd = usb_receive_buf[n];
+ //pc.printf("..valid command is [%d]\r\n",n);
+ }
+ //else{ pc.printf("..but [%d] not a valid command\r\n",n);}
+ //startflag will remain true if it's in any
+ //of the lookup tables. Make a copy of the
+ //valid command.
+ }
+ //else{ pc.printf("Previous byte wasnt start byte\r\n");}
+
+ if((usb_receive_buf[n] == _MSG_END) &&
+ (start_flag)){
+ //pc.printf("'end' found @ [%d]\r\n",n);
+ //If startflag is still true and we find an
+ //end character then check len(data bytes).
+ temp_end = n;
+ //pc.printf("temp_end = [%d]\r\n",n);
+ //uint8_t j_f_test = c_looklen(temp_cmd);
+ //pc.printf("should have %d data bytes\r\n",j_f_test);
+ if(((temp_end - temp_pnt)-2) ==
+ c_looklen(temp_cmd)) { chk_pnt = n+1;
+ //pc.printf("..does have %d bytes\r\n",j_f_test);
+ }
+ //If the message is correct size we have to
+ //check the next byte is a check byte.
+ else { chk_pnt = 0;
+ //j_f_test = ((temp_end - temp_pnt)-2);
+ //pc.printf("..but instead has %d bytes\r\n",j_f_test);
+ }
+ }
+ //else{ pc.printf("[%d] not an end byte\r\n",n);}
+
+ if((n == chk_pnt) && ((n-1) == temp_end)){
+ //pc.printf("[%d] should be check byte\r\n",n);
+ if(check_msg(temp_pnt, chk_pnt)){
+ //pc.printf("..the check byte is valid");
+ //save the data bytes somewhere
+ //pc.printf("*need to save data bytes somewhere*\r\n");
+ switch(temp_cmd) {
+ case 'f': {
+ c_data_0x66[0] = usb_receive_buf[n-4];
+ c_data_0x66[1] = usb_receive_buf[n-3];
+ c_data_0x66[2] = usb_receive_buf[n-2];
+ command_clean(temp_pnt, chk_pnt);
+ if((c_data_0x66[0] == '2') && (c_data_0x66[1] == '9'))
+ { fdigi_out(29,(c_data_0x66[2]-48)); }
+ if((c_data_0x66[0] == '3') && (c_data_0x66[1] == '0'))
+ { fdigi_out(30,(c_data_0x66[2]-48)); }
+ //pc.printf("%c%c%c",_MBED_MSG_START,temp_cmd,_MSG_END);
+ stan_reply(temp_cmd);
+ break;
+ }
+ case 's': {
+ c_data_0x73[0] = usb_receive_buf[n-3];
+ c_data_0x73[1] = usb_receive_buf[n-2];
+ command_clean(temp_pnt, chk_pnt);
+ sdigi_out(c_data_0x73[0], (c_data_0x73[1]-48));
+ stan_reply(temp_cmd);
+ break;
+ }
+ case 'i': {
+ command_clean(temp_pnt, chk_pnt);
+ pc.printf("\r\n");
+ boot_msg();
+ stan_reply(temp_cmd);
+ break;
+ }
+ case 'y': {
+ command_clean(temp_pnt, chk_pnt);
+ int freq = directsync();
+ set_discfreq(freq);
+ freq_reply(freq, temp_cmd);
+ break;
+ }
+ case 'p': {
+ command_clean(temp_pnt, chk_pnt);
+ int dfreq = get_disc_freq();
+ freq_reply(dfreq, temp_cmd);
+ break;
+ }
+ case 'm': {
+ c_data_0x6D[0] = (usb_receive_buf[n-3]-48);
+ c_data_0x6D[1] = (usb_receive_buf[n-2]-48);
+ command_clean(temp_pnt, chk_pnt);
+ set_mux_chan(c_data_0x6D[0],c_data_0x6D[1]);
+ stan_reply(temp_cmd);
+ break;
+ }
+ case 'd': {
+ command_clean(temp_pnt, chk_pnt);
+ diag_reply();
+ break;
+ }
+ case 'r': {
+ c_data_0x72[0] = (usb_receive_buf[n-4]-48);
+ c_data_0x72[1] = (usb_receive_buf[n-3]-48);
+ c_data_0x72[2] = (usb_receive_buf[n-2]-48);
+ command_clean(temp_pnt, chk_pnt);
+ float speedval = ((c_data_0x72[1]*10) + c_data_0x72[2]);
+ speedval = speedval / 100;
+ set_servo(c_data_0x72[0], speedval);
+ stan_reply(temp_cmd);
+ break;
+ }
+ case 'q': {
+ c_data_0x71[0] = (usb_receive_buf[n-4]-48);
+ c_data_0x71[1] = (usb_receive_buf[n-3]-48);
+ c_data_0x71[2] = (usb_receive_buf[n-2]-48);
+ command_clean(temp_pnt, chk_pnt);
+ int discrpm = (c_data_0x71[1]*10) + c_data_0x71[2];
+ discrpm = discrpm + (c_data_0x71[0]*100);
+ set_discfreq(discrpm);
+ stan_reply(temp_cmd);
+ break;
+ }
+ case 't': {
+ command_clean(temp_pnt, chk_pnt);
+ stan_reply(temp_cmd);
+ wait(5);
+ mbed_reset();
+ break;
+ }
+ case 'v': {
+ command_clean(temp_pnt, chk_pnt);
+ volts_IR();
+ break;
+ }
+ case 'o': {
+ c_data_0x6F[0] = usb_receive_buf[n-2];
+ command_clean(temp_pnt, chk_pnt);
+ rotate_scene_cont(c_data_0x6F[0]);
+ stan_reply(temp_cmd);
+ break;
+ }
+ case 'u': {
+ command_clean(temp_pnt, chk_pnt);
+ ultra_range();
+ break;
+ }
+ }
+ //pc.printf("'f' data bytes %c %c %c\r\n",c_data_0x66[0],c_data_0x66[1],c_data_0x66[2]);
+ //pc.printf("'b' data bytes %c %c\r\n",c_data_0x62[0],c_data_0x62[1]);
+ //pc.printf("'c' data bytes %c %c %c\r\n",c_data_0x63[0],c_data_0x63[1],c_data_0x63[2]);
+ ret_cmd = temp_cmd;
+ }
+ }
+ if(ret_cmd != 0){ break; }
+ }
+ //pc.printf("..returning ret_cmd\r\n");
+ return ret_cmd;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "dio_setup" : Function to set up the standard digi IO
+void dio_setup (void){
+ p29_out = _def_p29_out;
+ p30_out = _def_p30_out;
+ //disc0_out = _def_disc0;
+ //disc1_out = _def_disc1;
+ disc2_out = _def_disc2;
+ //sync_out = _def_sync;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "servo_setup" : Function to set up the standard servos
+void servo_setup (void){
+ s_ervo1 = _def_servo1;
+ s_ervo2 = _def_servo2;
+ s_ervo3 = _def_servo3;
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "usb_serial_setup" : Function to set up the USB serial
+// interface.
+void usb_serial_setup (void){
+ uint8_t n;
+ for(n=0;n<_USB_RECEIVE_BUF;n++){ usb_receive_buf[n]=0x00;}
+ usb_receive_pnt = 0; //clr buffer & setup pointer
+ usb_receive_flg = false;//clr the byte rec'd flag
+ pc.baud(_USB_BAUDRATE); //set the baudrate
+ pc.attach(&usb_serial_r);; //configure the ISR
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "scan_1s" : Function to check the recieve buffer
+// periodically (1s) to make sure there are no dormant
+// commands left in there.
+void scan_1s(void){
+ led1 = 1; //heartbeat
+ if(!check_usb_rec_flg()){
+ uint8_t holder = usb_serial_bufscan();
+ while( holder != 0 ){
+ holder = usb_serial_bufscan();
+ }
+ }
+ wait_us(10); led1 = 0; //heartbeat
+}
+/////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////
+// "startup_config" : Function to set up all the defaults
+// for the system.
+void startup_config (void){
+ led1 = 0;
+ usb_serial_setup(); //set up serial interface
+ pc.printf(_BOOT_MESSAGE);
+ pc.printf(" USBSERIAL:");
+ pc.printf("%d:8:N:1\r\n",_USB_BAUDRATE);
+ boot_msg(); //display the boot message
+ dio_setup(); //set up standard digital IO
+ servo_setup(); //set up standard servos
+ uint16_t sdata_out = 0x0000; //clear the output shifter
+ sdata_out = dshift_combine(); //combine into 16bits
+ dshift_out(sdata_out); //perform the shift
+ pc.printf(" FDIGIOUT");
+ pc.printf(":%d",_def_p29_out);
+ pc.printf(":%d",_def_p30_out);
+ pc.printf("\r\n");
+ pc.printf(" SDIGIOUT");
+ pc.printf(":%d",_def_sdigioutA);
+ pc.printf(":%d",_def_sdigioutB);
+ pc.printf(":%d",_def_sdigioutC);
+ pc.printf(":%d",_def_sdigioutD);
+ pc.printf(":%d",_def_sdigioutE);
+ pc.printf(":%d",_def_sdigioutF);
+ pc.printf("\r\n");
+ pc.printf(" MUXCONF");
+ pc.printf(":%d",_def_mux00_ch);
+ pc.printf(":%d",_def_mux01_ch);
+ pc.printf(":%d",_def_mux02_ch);
+ pc.printf(":%d",_def_mux03_ch);
+ pc.printf(":%d",_def_mux04_ch);
+ pc.printf("\r\n");
+ pc.printf(" SERVO");
+ pc.printf(":%d",(_def_servo1*100));
+ pc.printf(":%d",(_def_servo2*100));
+ pc.printf(":%d",(_def_servo3*100));
+ pc.printf("\r\n");
+ scan.attach(&scan_1s, 1.0); // setup "scan_1s" to run every 1.0s
+}
+/////////////////////////////////////////////////////////
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/daughter_core.h Wed Aug 10 08:01:00 2016 +0000 @@ -0,0 +1,217 @@ +// This header file is for the core firmware functions +// associated with the mbed daughter board. + +///////////////////////////////////////////////////////// +// "boot_msg" : Function to send a bootup message via the +// usb serial link. +void boot_msg(void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "scan_1s" : Function to check the recieve buffer +// periodically (1s) to make sure there are no dormant +// commands left in there. +void scan_1s(void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "command_clean" : Function to clean a command from the +// recieve buffer quickly. +void command_clean(uint8_t t_pnt, uint8_t c_pnt); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "diag_reply" : Function to send diagnostics info back +// to the host PC via the USB serial. +void diag_reply(void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "dshift_out" : Function to shift data out via the +// shift register. +// This will effect the digital shift outputs and the +// multiplexer configuration. +void dshift_out(uint16_t sdata_out); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "freq_reply" : Function send a reply message back to +// the host PC to say which frequency was detected on +// the directsync pin. This will have been applied to the +// motordrive also. +void freq_reply(int freq, uint8_t command); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "volts_IR" : Function reads the analogue input voltage +// present on the directsync_in pin +void volts_IR(void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "ultra_range" : Function performs an ultrasonic range +// measurement and outputs the duration of the sensor +// output pulse. +void ultra_range(void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "directsync" : Function read the input frequency on +// the direct sync pin. This frequency should then be +// applied to the motor drive. This routine will then +// return the frequency it applied. +int directsync(void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "get_disc_freq" : Function read the frequency of the +// spinning disc. This routine will then +// return the frequency measured. +int get_disc_freq(void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "set_servo" : Function sets the servo speed as a +// floating point fraction of 100%. +void set_servo(uint8_t servo, float speedval); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "rotate_scene_cont" : Function to turn ranging scene +// to the left or the right by a small amount +void rotate_scene_cont(uint8_t direction); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "rotate_scene" : Function to turn ranging scene +// to the left or the right by a small amount +void rotate_scene(uint8_t direction); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "set_discfreq" : Function attempts to set the disc +// rpm to the defined value. When rpm is reached it stops +// trying. +void set_discfreq(int discrpm); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "dshift_combine" : Function to combine all shift reg +// elements into a single 16bit variable, for shifting. +// This will return the 16bit variable. +uint16_t dshift_combine(void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "set_mux_chan" : Function to configure one of the five +// daughter board multiplexers. (0 to 4). Each multiplexer +// can select channels 1,2,3 or high-impedance (0). +// This will effect the multiplexer configuration. +void set_mux_chan(uint8_t mux, uint8_t chan); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "print_array" : Function to send all character bytes +// from an array out through the serial port. +void print_array(char *array, uint8_t size); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "stan_reply" : Function to send a standard reply from +// the mBed to the Host PC. A standard reply is one that +// confirms that the message sent by the host has been +// actioned. +void stan_reply(char command); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "fdigi_out" : Function to assert digital outputs +// attached directly to the mbed pins p29 and p30. +void fdigi_out(uint8_t pin, bool pinval); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "sdigi_out" : Function to assert digital outputs +// attached to the daughter board shift register. +// digi channels a, b, c, d, e and f. +// This will effect the digital shift outputs only. +void sdigi_out(char digi_ch, bool pinval); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "check_usb_rec_flg" : Function to see if any bytes +// have come in, on the USB serial. Returns the +// usb_receive_flg. +bool check_usb_rec_flg(void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "clr_usb_rec_flg" : Function to clear the +// usb_receive_flg. +void clr_usb_rec_flg(void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "usb_serial_r" : Function to deal with serial bytes +// being received via USB serial. When a byte is rec'd +// it is copied to a circular buffer and then a rec'd +// flag is asserted to show that action is needed. +void usb_serial_r (void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "usb_serial_t" : Function to send array of characters +// to the host PC via the usb serial. Returns the number +// of characters written to the USB serial. +uint8_t usb_serial_t (char txstring[]); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "c_lookchk" : Function to check to see if the command +// byte exists in any of the lookup tables. If it does +// then true is returned. +bool c_lookchk (uint8_t command); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "c_looklen" : Function to lookup the defined length of +// the given command's data bytes. Returns the number of +// data bytes that should exist. +uint8_t c_looklen (uint8_t command); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "check_msg" : That generates the final check byte from +// the message found between tmp_pnt and chk_pnt. If the +// check byte matches then true is returned. +bool check_msg(uint8_t tmp_pnt, uint8_t chk_pnt); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "usb_serial_bufscan" : Function to check the INPUT +// BUFFER to see if a valid command has been received. If +// so then the function returns with the command ID. +uint8_t usb_serial_bufscan (void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "usb_serial_setup" : Function to set up the USB serial +// interface. +void usb_serial_setup (void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "dio_setup" : Function to set up the standard digi IO +void dio_setup (void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "servo_setup" : Function to set up the standard servos +void servo_setup (void); +///////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////// +// "startup_config" : Function to set up all the defaults +// for the system. +void startup_config (void); +///////////////////////////////////////////////////////// \ No newline at end of file
--- a/main.cpp Fri Mar 01 13:33:02 2013 +0000
+++ b/main.cpp Wed Aug 10 08:01:00 2016 +0000
@@ -1,14 +1,21 @@
-#include "mbed.h"
-#include "USBSerial.h"
-
-//Virtual serial port over USB
-USBSerial serial;
+#include "core_defines.h" //for all global constant definitions
+#include "mbed.h" //to include the mBed library
+#include "Servo.h" //to include the mBed Servo library
+#include "daughter_core.h" //for all daughter board core functions
+
-int main(void) {
-
- while(1)
- {
- serial.printf("I am a virtual serial port\r\n");
- wait(1);
+int main() {
+ startup_config();
+ usb_serial_t("BOOT COMPLETE\r\n");
+
+ while(1) {
+ if(check_usb_rec_flg()){
+
+ //char comm[1];
+ //comm[0] = usb_serial_bufscan();
+ //usb_serial_t(comm);
+ usb_serial_bufscan();
+ clr_usb_rec_flg();
+ }
}
}
\ No newline at end of file