STEM ELF / Mbed 2 deprecated mbed_daugher_v0_ranging

This is the firmware for the base version of the OpenSTEM Ranging Board hardware.

Dependencies:   Servo USBDevice mbed

Fork of USBSerial_HelloWorld by Samuel Mokrani

daughter_core.cpp@10:d74124f266cb, 2016-08-10 (annotated)

Committer:
matthewgascoyne
Date:
Wed Aug 10 08:01:00 2016 +0000
Revision:
10:d74124f266cb
base version of OpenSTEM Ranging Board Firmware

Who changed what in which revision?

UserRevisionLine numberNew contents of line
matthewgascoyne 10:d74124f266cb 1 // This include file is for the core firmware functions
matthewgascoyne 10:d74124f266cb 2 // associated with the mbed daughter board.
matthewgascoyne 10:d74124f266cb 3
matthewgascoyne 10:d74124f266cb 4 #include "core_defines.h"
matthewgascoyne 10:d74124f266cb 5 #include "command_matrix.h"
matthewgascoyne 10:d74124f266cb 6 #include "stdint.h"
matthewgascoyne 10:d74124f266cb 7 #include "Servo.h" //to include the mBed Servo library
matthewgascoyne 10:d74124f266cb 8 #include "mbed.h"
matthewgascoyne 10:d74124f266cb 9
matthewgascoyne 10:d74124f266cb 10 extern DigitalOut shift_data(p26); // These pins are used exclusively by
matthewgascoyne 10:d74124f266cb 11 extern DigitalOut shift_srclk(p25); // the "dshift_out" function.
matthewgascoyne 10:d74124f266cb 12 extern DigitalOut shift_rclk(p24); //
matthewgascoyne 10:d74124f266cb 13
matthewgascoyne 10:d74124f266cb 14 extern char unique_nm[8] = {0,0,0,0,0,0,0,0}; //if user sets unique name
matthewgascoyne 10:d74124f266cb 15 extern char daughter_nm[8] = {0,0,0,0,0,0,0,0}; //if user sets daughter name
matthewgascoyne 10:d74124f266cb 16 extern char parent_nm[8] = {0,0,0,0,0,0,0,0}; //if user sets parent name
matthewgascoyne 10:d74124f266cb 17
matthewgascoyne 10:d74124f266cb 18 extern "C" void mbed_reset(); // used to remotely reset the mbed
matthewgascoyne 10:d74124f266cb 19 Timer direct_s_tim; // Set up timer for direct sync detection
matthewgascoyne 10:d74124f266cb 20 Timer range_tim; // Set up timer for ultrasonic ranging
matthewgascoyne 10:d74124f266cb 21 Ticker scan; // To make sure no dormant commands in buffer
matthewgascoyne 10:d74124f266cb 22 DigitalOut led1(LED1); // set up the heartbeat LED1
matthewgascoyne 10:d74124f266cb 23
matthewgascoyne 10:d74124f266cb 24 #if _p29_IO_Dir
matthewgascoyne 10:d74124f266cb 25 DigitalOut p29_out(p29);
matthewgascoyne 10:d74124f266cb 26 #else
matthewgascoyne 10:d74124f266cb 27 DigitalIn p29_in(p29);
matthewgascoyne 10:d74124f266cb 28 #endif
matthewgascoyne 10:d74124f266cb 29
matthewgascoyne 10:d74124f266cb 30 #if _p30_IO_Dir
matthewgascoyne 10:d74124f266cb 31 DigitalOut p30_out(p30);
matthewgascoyne 10:d74124f266cb 32 #else
matthewgascoyne 10:d74124f266cb 33 DigitalIn p30_in(p30);
matthewgascoyne 10:d74124f266cb 34 #endif
matthewgascoyne 10:d74124f266cb 35
matthewgascoyne 10:d74124f266cb 36 #if _disc0_Dir //if configured as output
matthewgascoyne 10:d74124f266cb 37 DigitalOut disc0_out(p15);
matthewgascoyne 10:d74124f266cb 38 #else //if configured as input
matthewgascoyne 10:d74124f266cb 39 #if _disc0_AD //if input & analogue
matthewgascoyne 10:d74124f266cb 40 AnalogIn disc0_in(p15);
matthewgascoyne 10:d74124f266cb 41 #else //otherwise input & digital
matthewgascoyne 10:d74124f266cb 42 DigitalIn disc0_in(p15);
matthewgascoyne 10:d74124f266cb 43 #endif
matthewgascoyne 10:d74124f266cb 44 #endif
matthewgascoyne 10:d74124f266cb 45
matthewgascoyne 10:d74124f266cb 46 #if _disc1_Dir //if configured as output
matthewgascoyne 10:d74124f266cb 47 DigitalOut disc1_out(p16);
matthewgascoyne 10:d74124f266cb 48 #else //if configured as input
matthewgascoyne 10:d74124f266cb 49 #if _disc1_AD //if input & analogue
matthewgascoyne 10:d74124f266cb 50 AnalogIn disc1_in(p16);
matthewgascoyne 10:d74124f266cb 51 #else //otherwise input & digital
matthewgascoyne 10:d74124f266cb 52 DigitalIn disc1_in(p16);
matthewgascoyne 10:d74124f266cb 53 #endif
matthewgascoyne 10:d74124f266cb 54 #endif
matthewgascoyne 10:d74124f266cb 55
matthewgascoyne 10:d74124f266cb 56 #if _disc2_Dir //if configured as output
matthewgascoyne 10:d74124f266cb 57 DigitalOut disc2_out(p17);
matthewgascoyne 10:d74124f266cb 58 #else //if configured as input
matthewgascoyne 10:d74124f266cb 59 #if _disc2_AD //if input & analogue
matthewgascoyne 10:d74124f266cb 60 AnalogIn disc2_in(p17);
matthewgascoyne 10:d74124f266cb 61 #else //otherwise input & digital
matthewgascoyne 10:d74124f266cb 62 DigitalIn disc2_in(p17);
matthewgascoyne 10:d74124f266cb 63 #endif
matthewgascoyne 10:d74124f266cb 64 #endif
matthewgascoyne 10:d74124f266cb 65
matthewgascoyne 10:d74124f266cb 66 #if _sync_Dir //if configured as output
matthewgascoyne 10:d74124f266cb 67 #if _sync_AD //if output & analogue
matthewgascoyne 10:d74124f266cb 68 AnalogOut sync_out(p18);
matthewgascoyne 10:d74124f266cb 69 #else //if output & digital
matthewgascoyne 10:d74124f266cb 70 DigitalOut sync_out(p18);
matthewgascoyne 10:d74124f266cb 71 #endif
matthewgascoyne 10:d74124f266cb 72 #else //if configured as input
matthewgascoyne 10:d74124f266cb 73 #if _sync_AD //if input and analogue
matthewgascoyne 10:d74124f266cb 74 AnalogIn sync_in(p18);
matthewgascoyne 10:d74124f266cb 75 #else //if input and digital
matthewgascoyne 10:d74124f266cb 76 DigitalIn sync_in(p18);
matthewgascoyne 10:d74124f266cb 77 #endif
matthewgascoyne 10:d74124f266cb 78 #endif
matthewgascoyne 10:d74124f266cb 79
matthewgascoyne 10:d74124f266cb 80 #if _directsync_AD //if input & analogue
matthewgascoyne 10:d74124f266cb 81 AnalogIn directsync_in(p19);
matthewgascoyne 10:d74124f266cb 82 #else //if input and digital
matthewgascoyne 10:d74124f266cb 83 DigitalIn directsync_in(p19);
matthewgascoyne 10:d74124f266cb 84 #endif
matthewgascoyne 10:d74124f266cb 85
matthewgascoyne 10:d74124f266cb 86 #if _fdetect_AD //if input and analogue
matthewgascoyne 10:d74124f266cb 87 AnalogIn fdetect_in(p20);
matthewgascoyne 10:d74124f266cb 88 #else //if input and digital
matthewgascoyne 10:d74124f266cb 89 DigitalIn fdetect_in(p20);
matthewgascoyne 10:d74124f266cb 90 #endif
matthewgascoyne 10:d74124f266cb 91
matthewgascoyne 10:d74124f266cb 92 Servo s_ervo1(p23);
matthewgascoyne 10:d74124f266cb 93 Servo s_ervo2(p22);
matthewgascoyne 10:d74124f266cb 94 Servo s_ervo3(p21);
matthewgascoyne 10:d74124f266cb 95
matthewgascoyne 10:d74124f266cb 96 extern uint8_t mux00_ch = _def_mux00_ch;//channel select for Mux00 (0,1,2 or 3)
matthewgascoyne 10:d74124f266cb 97 extern uint8_t mux01_ch = _def_mux01_ch;//channel select for Mux01 (0,1,2 or 3)
matthewgascoyne 10:d74124f266cb 98 extern uint8_t mux02_ch = _def_mux02_ch;//channel select for Mux02 (0,1,2 or 3)
matthewgascoyne 10:d74124f266cb 99 extern uint8_t mux03_ch = _def_mux03_ch;//channel select for Mux03 (0,1,2 or 3)
matthewgascoyne 10:d74124f266cb 100 extern uint8_t mux04_ch = _def_mux04_ch;//channel select for Mux04 (0,1,2 or 3)
matthewgascoyne 10:d74124f266cb 101
matthewgascoyne 10:d74124f266cb 102 extern bool sdigi_a = _def_sdigioutA; //shift digital channel output a (0 or 1)
matthewgascoyne 10:d74124f266cb 103 extern bool sdigi_b = _def_sdigioutB; //shift digital channel output b (0 or 1)
matthewgascoyne 10:d74124f266cb 104 extern bool sdigi_c = _def_sdigioutC; //shift digital channel output c (0 or 1)
matthewgascoyne 10:d74124f266cb 105 extern bool sdigi_d = _def_sdigioutD; //shift digital channel output d (0 or 1)
matthewgascoyne 10:d74124f266cb 106 extern bool sdigi_e = _def_sdigioutE; //shift digital channel output e (0 or 1)
matthewgascoyne 10:d74124f266cb 107 extern bool sdigi_f = _def_sdigioutF; //shift digital channel output f (0 or 1)
matthewgascoyne 10:d74124f266cb 108
matthewgascoyne 10:d74124f266cb 109 extern Serial pc(USBTX, USBRX); //the USB serial channel
matthewgascoyne 10:d74124f266cb 110 uint8_t usb_receive_buf[_USB_RECEIVE_BUF]; //setup serial receive buffer
matthewgascoyne 10:d74124f266cb 111 extern uint8_t usb_receive_pnt = 0; //pointer to position in buffer
matthewgascoyne 10:d74124f266cb 112 extern bool usb_receive_flg = false; //true=new byte, false=NO new byte
matthewgascoyne 10:d74124f266cb 113
matthewgascoyne 10:d74124f266cb 114 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 115 // "print_array" : Function to send all character bytes
matthewgascoyne 10:d74124f266cb 116 // from an array out through the serial port.
matthewgascoyne 10:d74124f266cb 117 void print_array(char *array, uint8_t size){
matthewgascoyne 10:d74124f266cb 118 for (uint8_t n = 0; n < size; n++){
matthewgascoyne 10:d74124f266cb 119 while((!pc.writeable())){wait_us(_USB_TX_WAIT_US);}
matthewgascoyne 10:d74124f266cb 120 if(array[n] == '\0'){ break; }
matthewgascoyne 10:d74124f266cb 121 pc.putc(array[n]);
matthewgascoyne 10:d74124f266cb 122 }
matthewgascoyne 10:d74124f266cb 123 }
matthewgascoyne 10:d74124f266cb 124 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 125
matthewgascoyne 10:d74124f266cb 126 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 127 // "boot_msg" : Function to send a bootup message via the
matthewgascoyne 10:d74124f266cb 128 // usb serial link.
matthewgascoyne 10:d74124f266cb 129 void boot_msg(void){
matthewgascoyne 10:d74124f266cb 130 pc.printf("F/W VERSION:%3.1f\r\n",_FW_VERSION);
matthewgascoyne 10:d74124f266cb 131 pc.printf("H/W VERSION:");
matthewgascoyne 10:d74124f266cb 132 pc.printf(_HW_VERSION);
matthewgascoyne 10:d74124f266cb 133 pc.printf("\r\n");
matthewgascoyne 10:d74124f266cb 134 pc.printf(" UNIQUE:");
matthewgascoyne 10:d74124f266cb 135 if(unique_nm[0] != 0){
matthewgascoyne 10:d74124f266cb 136 print_array(unique_nm, sizeof(unique_nm)); }
matthewgascoyne 10:d74124f266cb 137 else { pc.printf(_UNIQUE_NAME); }
matthewgascoyne 10:d74124f266cb 138 pc.printf("\r\n");
matthewgascoyne 10:d74124f266cb 139 pc.printf(" DAUGHTER:");
matthewgascoyne 10:d74124f266cb 140 if(daughter_nm[0] != 0){
matthewgascoyne 10:d74124f266cb 141 print_array(daughter_nm, sizeof(daughter_nm)); }
matthewgascoyne 10:d74124f266cb 142 else { pc.printf(_DAUGHTER_NAME); }
matthewgascoyne 10:d74124f266cb 143 pc.printf("\r\n");
matthewgascoyne 10:d74124f266cb 144 pc.printf(" PARENT:");
matthewgascoyne 10:d74124f266cb 145 if(parent_nm[0] != 0){
matthewgascoyne 10:d74124f266cb 146 print_array(parent_nm, sizeof(parent_nm)); }
matthewgascoyne 10:d74124f266cb 147 else { pc.printf(_PARENT_NAME); }
matthewgascoyne 10:d74124f266cb 148 pc.printf("\r\n");
matthewgascoyne 10:d74124f266cb 149 }
matthewgascoyne 10:d74124f266cb 150 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 151
matthewgascoyne 10:d74124f266cb 152 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 153 // "command_clean" : Function to clean a command from the
matthewgascoyne 10:d74124f266cb 154 // recieve buffer quickly.
matthewgascoyne 10:d74124f266cb 155 void command_clean(uint8_t t_pnt, uint8_t c_pnt){
matthewgascoyne 10:d74124f266cb 156 //remove command from buffer
matthewgascoyne 10:d74124f266cb 157 for(uint8_t m=t_pnt; m<=c_pnt; m++){
matthewgascoyne 10:d74124f266cb 158 usb_receive_buf[m] = 0x00;
matthewgascoyne 10:d74124f266cb 159 }
matthewgascoyne 10:d74124f266cb 160 }
matthewgascoyne 10:d74124f266cb 161 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 162
matthewgascoyne 10:d74124f266cb 163 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 164 // "diag_reply" : Function to send diagnostics info back
matthewgascoyne 10:d74124f266cb 165 // to the host PC via the USB serial.
matthewgascoyne 10:d74124f266cb 166 void diag_reply(void){
matthewgascoyne 10:d74124f266cb 167 char stream[21] = {
matthewgascoyne 10:d74124f266cb 168 _MBED_MSG_START, 'd',
matthewgascoyne 10:d74124f266cb 169 'M', (mux00_ch+48) , (mux01_ch+48) , (mux02_ch+48) ,
matthewgascoyne 10:d74124f266cb 170 (mux03_ch+48) , (mux04_ch+48) ,
matthewgascoyne 10:d74124f266cb 171 'S', (sdigi_a+48) , (sdigi_b+48) , (sdigi_c+48) ,
matthewgascoyne 10:d74124f266cb 172 (sdigi_d+48) , (sdigi_e+48) , (sdigi_f+48) ,
matthewgascoyne 10:d74124f266cb 173 'F', (p29_out+48) , (p30_out+48) ,
matthewgascoyne 10:d74124f266cb 174 _MSG_END , 0x00 , '\0' ,};
matthewgascoyne 10:d74124f266cb 175 uint8_t size = ((sizeof(stream))-2);
matthewgascoyne 10:d74124f266cb 176 for(uint8_t n=0; n<size; n++){
matthewgascoyne 10:d74124f266cb 177 stream[size] = stream[size] ^ stream[n];
matthewgascoyne 10:d74124f266cb 178 }
matthewgascoyne 10:d74124f266cb 179 if(stream[size] == _HOST_MSG_START){stream[size]++;}
matthewgascoyne 10:d74124f266cb 180 if(stream[size] == _MBED_MSG_START){stream[size]++;}
matthewgascoyne 10:d74124f266cb 181 if(stream[size] == _MSG_END){stream[size]++;}
matthewgascoyne 10:d74124f266cb 182 print_array(stream, sizeof(stream));
matthewgascoyne 10:d74124f266cb 183 }
matthewgascoyne 10:d74124f266cb 184 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 185
matthewgascoyne 10:d74124f266cb 186 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 187 // "dshift_out" : Function to shift data out via the
matthewgascoyne 10:d74124f266cb 188 // shift register.
matthewgascoyne 10:d74124f266cb 189 // This will effect the digital shift outputs and the
matthewgascoyne 10:d74124f266cb 190 // multiplexer configuration.
matthewgascoyne 10:d74124f266cb 191 void dshift_out(uint16_t sdata_out) {
matthewgascoyne 10:d74124f266cb 192 shift_data = 0; shift_srclk = 0; shift_rclk = 0;
matthewgascoyne 10:d74124f266cb 193 wait(0.005);
matthewgascoyne 10:d74124f266cb 194 for(char n=0; n<16; n++)
matthewgascoyne 10:d74124f266cb 195 {
matthewgascoyne 10:d74124f266cb 196 if(1<<n & sdata_out)
matthewgascoyne 10:d74124f266cb 197 {
matthewgascoyne 10:d74124f266cb 198 shift_data = 1;
matthewgascoyne 10:d74124f266cb 199 }
matthewgascoyne 10:d74124f266cb 200 else
matthewgascoyne 10:d74124f266cb 201 {
matthewgascoyne 10:d74124f266cb 202 shift_data = 0;
matthewgascoyne 10:d74124f266cb 203 }
matthewgascoyne 10:d74124f266cb 204 wait(0.0001);
matthewgascoyne 10:d74124f266cb 205 shift_srclk = 1;
matthewgascoyne 10:d74124f266cb 206 wait(0.0001);
matthewgascoyne 10:d74124f266cb 207 shift_srclk = 0;
matthewgascoyne 10:d74124f266cb 208 }
matthewgascoyne 10:d74124f266cb 209 wait(0.0001);
matthewgascoyne 10:d74124f266cb 210 shift_rclk = 1;
matthewgascoyne 10:d74124f266cb 211 wait(0.0001);
matthewgascoyne 10:d74124f266cb 212 shift_rclk = 0;
matthewgascoyne 10:d74124f266cb 213 }
matthewgascoyne 10:d74124f266cb 214 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 215
matthewgascoyne 10:d74124f266cb 216 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 217 // "set_servo" : Function sets the servo speed as a
matthewgascoyne 10:d74124f266cb 218 // floating point fraction of 100%.
matthewgascoyne 10:d74124f266cb 219 void set_servo(uint8_t servo, float speedval){
matthewgascoyne 10:d74124f266cb 220 switch(servo){
matthewgascoyne 10:d74124f266cb 221 case 1: s_ervo1 = speedval; break;
matthewgascoyne 10:d74124f266cb 222 case 2: s_ervo2 = speedval; break;
matthewgascoyne 10:d74124f266cb 223 case 3: s_ervo3 = speedval; break;
matthewgascoyne 10:d74124f266cb 224 }
matthewgascoyne 10:d74124f266cb 225 }
matthewgascoyne 10:d74124f266cb 226 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 227
matthewgascoyne 10:d74124f266cb 228 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 229 // "freq_reply" : Function send a reply message back to
matthewgascoyne 10:d74124f266cb 230 // the host PC to say which frequency was detected on
matthewgascoyne 10:d74124f266cb 231 // the directsync pin. This will have been applied to the
matthewgascoyne 10:d74124f266cb 232 // motordrive also.
matthewgascoyne 10:d74124f266cb 233 void freq_reply(int freq, uint8_t command){
matthewgascoyne 10:d74124f266cb 234 char stream[8] = {_MBED_MSG_START , command ,
matthewgascoyne 10:d74124f266cb 235 '0' , '0' , '0' , _MSG_END , 0 , '\0' ,};
matthewgascoyne 10:d74124f266cb 236 stream[2] = (freq/100);
matthewgascoyne 10:d74124f266cb 237 stream[3] = (freq - (stream[2]*100))/10;
matthewgascoyne 10:d74124f266cb 238 stream[4] = (freq - ((stream[2]*100)+(stream[3]*10)));
matthewgascoyne 10:d74124f266cb 239 stream[2] = stream[2] + 48;
matthewgascoyne 10:d74124f266cb 240 stream[3] = stream[3] + 48;
matthewgascoyne 10:d74124f266cb 241 stream[4] = stream[4] + 48;
matthewgascoyne 10:d74124f266cb 242 uint8_t size = ((sizeof(stream))-2);
matthewgascoyne 10:d74124f266cb 243 for(uint8_t n=0; n<size; n++){
matthewgascoyne 10:d74124f266cb 244 stream[size] = stream[size] ^ stream[n];
matthewgascoyne 10:d74124f266cb 245 }
matthewgascoyne 10:d74124f266cb 246 if(stream[size] == _HOST_MSG_START){stream[size]++;}
matthewgascoyne 10:d74124f266cb 247 if(stream[size] == _MBED_MSG_START){stream[size]++;}
matthewgascoyne 10:d74124f266cb 248 if(stream[size] == _MSG_END){stream[size]++;}
matthewgascoyne 10:d74124f266cb 249 print_array(stream, sizeof(stream));
matthewgascoyne 10:d74124f266cb 250 //pc.printf("%d\r\n",freq);
matthewgascoyne 10:d74124f266cb 251 }
matthewgascoyne 10:d74124f266cb 252 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 253
matthewgascoyne 10:d74124f266cb 254 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 255 // "volts_IR" : Function read the analogue input voltage
matthewgascoyne 10:d74124f266cb 256 // present on the directsync_in pin
matthewgascoyne 10:d74124f266cb 257 void volts_IR(void){
matthewgascoyne 10:d74124f266cb 258 float the_volts = directsync_in;
matthewgascoyne 10:d74124f266cb 259 the_volts = the_volts * 3.3f * 100;
matthewgascoyne 10:d74124f266cb 260 int rounder = the_volts;
matthewgascoyne 10:d74124f266cb 261 float fraction = the_volts - rounder;
matthewgascoyne 10:d74124f266cb 262 //pc.printf(" %5.3f ", the_volts);
matthewgascoyne 10:d74124f266cb 263 //pc.printf(" %5.3f ", fraction);
matthewgascoyne 10:d74124f266cb 264 //pc.printf(" %d ", rounder);
matthewgascoyne 10:d74124f266cb 265 if(fraction >= 0.5f){ the_volts += 1; }
matthewgascoyne 10:d74124f266cb 266 char stream[8] = {_MBED_MSG_START , 'v' ,
matthewgascoyne 10:d74124f266cb 267 '0' , '0' , '0' , _MSG_END , 0 , '\0' ,};
matthewgascoyne 10:d74124f266cb 268 stream[2] = (the_volts / 100);
matthewgascoyne 10:d74124f266cb 269 stream[3] = (the_volts - (stream[2]*100)) / 10;
matthewgascoyne 10:d74124f266cb 270 stream[4] = the_volts - (stream[2]*100) - (stream[3]*10);
matthewgascoyne 10:d74124f266cb 271 stream[2] = stream[2] + 48;
matthewgascoyne 10:d74124f266cb 272 stream[3] = stream[3] + 48;
matthewgascoyne 10:d74124f266cb 273 stream[4] = stream[4] + 48;
matthewgascoyne 10:d74124f266cb 274 uint8_t size = ((sizeof(stream))-2);
matthewgascoyne 10:d74124f266cb 275 for(uint8_t n=0; n<size; n++){
matthewgascoyne 10:d74124f266cb 276 stream[size] = stream[size] ^ stream[n];
matthewgascoyne 10:d74124f266cb 277 }
matthewgascoyne 10:d74124f266cb 278 if(stream[size] == _HOST_MSG_START){stream[size]++;}
matthewgascoyne 10:d74124f266cb 279 if(stream[size] == _MBED_MSG_START){stream[size]++;}
matthewgascoyne 10:d74124f266cb 280 if(stream[size] == _MSG_END){stream[size]++;}
matthewgascoyne 10:d74124f266cb 281 print_array(stream, sizeof(stream));
matthewgascoyne 10:d74124f266cb 282 }
matthewgascoyne 10:d74124f266cb 283 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 284
matthewgascoyne 10:d74124f266cb 285 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 286 // "directsync" : Function read the input frequency on
matthewgascoyne 10:d74124f266cb 287 // the direct sync pin. This frequency should then be
matthewgascoyne 10:d74124f266cb 288 // applied to the motor drive. This routine will then
matthewgascoyne 10:d74124f266cb 289 // return the frequency it applied.
matthewgascoyne 10:d74124f266cb 290 int directsync(void){
matthewgascoyne 10:d74124f266cb 291 uint8_t fstate = 0;
matthewgascoyne 10:d74124f266cb 292 int freq = 0;
matthewgascoyne 10:d74124f266cb 293 uint16_t n;
matthewgascoyne 10:d74124f266cb 294 int begin = 0;
matthewgascoyne 10:d74124f266cb 295 int end = 1;
matthewgascoyne 10:d74124f266cb 296 for(n = 0; n<1000; n++){ //wait up to 250ms for
matthewgascoyne 10:d74124f266cb 297 if(directsync_in == 0){ //pin to be 0 (low)
matthewgascoyne 10:d74124f266cb 298 fstate = 1; break; }
matthewgascoyne 10:d74124f266cb 299 wait_us(250);
matthewgascoyne 10:d74124f266cb 300 }
matthewgascoyne 10:d74124f266cb 301 if(fstate == 0){ return 0;} //never went low so quit
matthewgascoyne 10:d74124f266cb 302 else{
matthewgascoyne 10:d74124f266cb 303 for(n = 0; n<2000; n++){ //now wait 100ms for pin
matthewgascoyne 10:d74124f266cb 304 if(directsync_in == 1){ //to be 1 (high)
matthewgascoyne 10:d74124f266cb 305 direct_s_tim.start();
matthewgascoyne 10:d74124f266cb 306 fstate = 2;
matthewgascoyne 10:d74124f266cb 307 begin = direct_s_tim.read_us(); break;
matthewgascoyne 10:d74124f266cb 308 } //if high, start timer
matthewgascoyne 10:d74124f266cb 309 wait_us(50);
matthewgascoyne 10:d74124f266cb 310 }
matthewgascoyne 10:d74124f266cb 311 }
matthewgascoyne 10:d74124f266cb 312 if(fstate != 2){ return 0; }//no high seen, then quit
matthewgascoyne 10:d74124f266cb 313 else{ //we saw a high so wait up
matthewgascoyne 10:d74124f266cb 314 for(n = 0; n<2000; n++){ //to 100ms for a (low)
matthewgascoyne 10:d74124f266cb 315 if(directsync_in == 0){
matthewgascoyne 10:d74124f266cb 316 fstate = 3; break;//sen low, so carry on
matthewgascoyne 10:d74124f266cb 317 }
matthewgascoyne 10:d74124f266cb 318 wait_us(50);
matthewgascoyne 10:d74124f266cb 319 }
matthewgascoyne 10:d74124f266cb 320 }
matthewgascoyne 10:d74124f266cb 321 if(fstate != 3){ return 0; } //no low see, so quit
matthewgascoyne 10:d74124f266cb 322 else{ //low seen so now we
matthewgascoyne 10:d74124f266cb 323 for(n = 0; n<2000; n++){ //wait up to 100ms for
matthewgascoyne 10:d74124f266cb 324 if(directsync_in == 1){ //the final high
matthewgascoyne 10:d74124f266cb 325 fstate = 4;
matthewgascoyne 10:d74124f266cb 326 end = direct_s_tim.read_us(); break;
matthewgascoyne 10:d74124f266cb 327 } //high seen, stop timer
matthewgascoyne 10:d74124f266cb 328 wait_us(50);
matthewgascoyne 10:d74124f266cb 329 }
matthewgascoyne 10:d74124f266cb 330 }
matthewgascoyne 10:d74124f266cb 331 direct_s_tim.stop();
matthewgascoyne 10:d74124f266cb 332 freq = 1000000 / (end - begin);
matthewgascoyne 10:d74124f266cb 333 return freq;
matthewgascoyne 10:d74124f266cb 334 }
matthewgascoyne 10:d74124f266cb 335 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 336
matthewgascoyne 10:d74124f266cb 337 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 338 // "get_disc_freq" : Function read the frequency of the
matthewgascoyne 10:d74124f266cb 339 // spinning disc. This routine will then
matthewgascoyne 10:d74124f266cb 340 // return the frequency measured.
matthewgascoyne 10:d74124f266cb 341 int get_disc_freq(void){
matthewgascoyne 10:d74124f266cb 342 uint8_t fstate = 0;
matthewgascoyne 10:d74124f266cb 343 int freq = 0;
matthewgascoyne 10:d74124f266cb 344 uint16_t n;
matthewgascoyne 10:d74124f266cb 345 int begin = 0;
matthewgascoyne 10:d74124f266cb 346 int end = 1;
matthewgascoyne 10:d74124f266cb 347 for(n = 0; n<1000; n++){ //wait up to 250ms for
matthewgascoyne 10:d74124f266cb 348 if(fdetect_in == 0){ //pin to be 0 (low)
matthewgascoyne 10:d74124f266cb 349 fstate = 1; break; }
matthewgascoyne 10:d74124f266cb 350 wait_us(250);
matthewgascoyne 10:d74124f266cb 351 }
matthewgascoyne 10:d74124f266cb 352 if(fstate == 0){ return 0;} //never went low so quit
matthewgascoyne 10:d74124f266cb 353 else{
matthewgascoyne 10:d74124f266cb 354 for(n = 0; n<2000; n++){ //now wait 100ms for pin
matthewgascoyne 10:d74124f266cb 355 if(fdetect_in == 1){ //to be 1 (high)
matthewgascoyne 10:d74124f266cb 356 direct_s_tim.start();
matthewgascoyne 10:d74124f266cb 357 fstate = 2;
matthewgascoyne 10:d74124f266cb 358 begin = direct_s_tim.read_us(); break;
matthewgascoyne 10:d74124f266cb 359 } //if high, start timer
matthewgascoyne 10:d74124f266cb 360 wait_us(50);
matthewgascoyne 10:d74124f266cb 361 }
matthewgascoyne 10:d74124f266cb 362 }
matthewgascoyne 10:d74124f266cb 363 if(fstate != 2){ return 0; }//no high seen, then quit
matthewgascoyne 10:d74124f266cb 364 else{ //we saw a high so wait up
matthewgascoyne 10:d74124f266cb 365 for(n = 0; n<2000; n++){ //to 100ms for a (low)
matthewgascoyne 10:d74124f266cb 366 if(fdetect_in == 0){
matthewgascoyne 10:d74124f266cb 367 fstate = 3; break;//sen low, so carry on
matthewgascoyne 10:d74124f266cb 368 }
matthewgascoyne 10:d74124f266cb 369 wait_us(50);
matthewgascoyne 10:d74124f266cb 370 }
matthewgascoyne 10:d74124f266cb 371 }
matthewgascoyne 10:d74124f266cb 372 if(fstate != 3){ return 0; } //no low see, so quit
matthewgascoyne 10:d74124f266cb 373 else{ //low seen so now we
matthewgascoyne 10:d74124f266cb 374 for(n = 0; n<2000; n++){ //wait up to 100ms for
matthewgascoyne 10:d74124f266cb 375 if(fdetect_in == 1){ //the final high
matthewgascoyne 10:d74124f266cb 376 fstate = 4;
matthewgascoyne 10:d74124f266cb 377 end = direct_s_tim.read_us(); break;
matthewgascoyne 10:d74124f266cb 378 } //high seen, stop timer
matthewgascoyne 10:d74124f266cb 379 wait_us(50);
matthewgascoyne 10:d74124f266cb 380 }
matthewgascoyne 10:d74124f266cb 381 }
matthewgascoyne 10:d74124f266cb 382 direct_s_tim.stop();
matthewgascoyne 10:d74124f266cb 383 freq = 1000000 / (end - begin);
matthewgascoyne 10:d74124f266cb 384 return freq;
matthewgascoyne 10:d74124f266cb 385 }
matthewgascoyne 10:d74124f266cb 386 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 387
matthewgascoyne 10:d74124f266cb 388 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 389 // "ultra_range" : Function performs an ultrasonic range
matthewgascoyne 10:d74124f266cb 390 // measurement and outputs the duration of the sensor
matthewgascoyne 10:d74124f266cb 391 // output pulse.
matthewgascoyne 10:d74124f266cb 392 void ultra_range(void){
matthewgascoyne 10:d74124f266cb 393 uint8_t rstate = 0;
matthewgascoyne 10:d74124f266cb 394 int begin = 0;
matthewgascoyne 10:d74124f266cb 395 int end = 1;
matthewgascoyne 10:d74124f266cb 396 float range = 0;
matthewgascoyne 10:d74124f266cb 397 p29_out = 0; //send the 50us
matthewgascoyne 10:d74124f266cb 398 p29_out = 1; //pulse to trigger
matthewgascoyne 10:d74124f266cb 399 wait_us(50); //the ultrasonic
matthewgascoyne 10:d74124f266cb 400 p29_out = 0; //
matthewgascoyne 10:d74124f266cb 401 range_tim.start(); //start the timer
matthewgascoyne 10:d74124f266cb 402 for(int n=0; n<1000; n++){
matthewgascoyne 10:d74124f266cb 403 if(fdetect_in == 1){
matthewgascoyne 10:d74124f266cb 404 begin = range_tim.read_us();
matthewgascoyne 10:d74124f266cb 405 rstate = 1;
matthewgascoyne 10:d74124f266cb 406 //pc.printf(" %d begin,\r\n",begin);
matthewgascoyne 10:d74124f266cb 407 break;
matthewgascoyne 10:d74124f266cb 408 }
matthewgascoyne 10:d74124f266cb 409 else {wait_us(1);}
matthewgascoyne 10:d74124f266cb 410 }
matthewgascoyne 10:d74124f266cb 411 if(rstate != 1){}
matthewgascoyne 10:d74124f266cb 412 else{
matthewgascoyne 10:d74124f266cb 413 for(uint16_t m=0; m<31000; m++){
matthewgascoyne 10:d74124f266cb 414 if(fdetect_in == 0){
matthewgascoyne 10:d74124f266cb 415 end = range_tim.read_us();
matthewgascoyne 10:d74124f266cb 416 rstate = 2;
matthewgascoyne 10:d74124f266cb 417 //pc.printf(" %d end.\r\n",end);
matthewgascoyne 10:d74124f266cb 418 break;
matthewgascoyne 10:d74124f266cb 419 }
matthewgascoyne 10:d74124f266cb 420 else {wait_us(1);}
matthewgascoyne 10:d74124f266cb 421 }
matthewgascoyne 10:d74124f266cb 422 }
matthewgascoyne 10:d74124f266cb 423 if(rstate != 2){}
matthewgascoyne 10:d74124f266cb 424 else{
matthewgascoyne 10:d74124f266cb 425 range_tim.stop();
matthewgascoyne 10:d74124f266cb 426 range_tim.reset();
matthewgascoyne 10:d74124f266cb 427 range = end - begin;
matthewgascoyne 10:d74124f266cb 428 range = (range / 5.80);
matthewgascoyne 10:d74124f266cb 429 //pc.printf(" %5.1fmm range.\r\n",range);
matthewgascoyne 10:d74124f266cb 430 }
matthewgascoyne 10:d74124f266cb 431 if(range > 999.0){ range = 999.0; }
matthewgascoyne 10:d74124f266cb 432 char stream[8] = {_MBED_MSG_START , 'u' ,
matthewgascoyne 10:d74124f266cb 433 '0' , '0' , '0' , _MSG_END , 0 , '\0' ,};
matthewgascoyne 10:d74124f266cb 434 stream[2] = (range / 100);
matthewgascoyne 10:d74124f266cb 435 stream[3] = (range - (stream[2]*100)) / 10;
matthewgascoyne 10:d74124f266cb 436 stream[4] = range - (stream[2]*100) - (stream[3]*10);
matthewgascoyne 10:d74124f266cb 437 stream[2] = stream[2] + 48;
matthewgascoyne 10:d74124f266cb 438 stream[3] = stream[3] + 48;
matthewgascoyne 10:d74124f266cb 439 stream[4] = stream[4] + 48;
matthewgascoyne 10:d74124f266cb 440 uint8_t size = ((sizeof(stream))-2);
matthewgascoyne 10:d74124f266cb 441 for(uint8_t n=0; n<size; n++){
matthewgascoyne 10:d74124f266cb 442 stream[size] = stream[size] ^ stream[n];
matthewgascoyne 10:d74124f266cb 443 }
matthewgascoyne 10:d74124f266cb 444 if(stream[size] == _HOST_MSG_START){stream[size]++;}
matthewgascoyne 10:d74124f266cb 445 if(stream[size] == _MBED_MSG_START){stream[size]++;}
matthewgascoyne 10:d74124f266cb 446 if(stream[size] == _MSG_END){stream[size]++;}
matthewgascoyne 10:d74124f266cb 447 print_array(stream, sizeof(stream));
matthewgascoyne 10:d74124f266cb 448 }
matthewgascoyne 10:d74124f266cb 449 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 450
matthewgascoyne 10:d74124f266cb 451 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 452 // "set_discfreq" : Function attempts to set the disc
matthewgascoyne 10:d74124f266cb 453 // rpm to the defined value. When rpm is reached it stops
matthewgascoyne 10:d74124f266cb 454 // trying.
matthewgascoyne 10:d74124f266cb 455 void set_discfreq(int discrpm){
matthewgascoyne 10:d74124f266cb 456 uint8_t count = 0;
matthewgascoyne 10:d74124f266cb 457 //float gain = 0.001;
matthewgascoyne 10:d74124f266cb 458 float newgain = 0.001;
matthewgascoyne 10:d74124f266cb 459 for(uint8_t n = 0; n<250; n++){
matthewgascoyne 10:d74124f266cb 460 int reading = get_disc_freq();
matthewgascoyne 10:d74124f266cb 461 newgain = (discrpm - reading);
matthewgascoyne 10:d74124f266cb 462 if(newgain > 40){ newgain = 40; }
matthewgascoyne 10:d74124f266cb 463 if(newgain < -40){ newgain = -40; }
matthewgascoyne 10:d74124f266cb 464 newgain = (newgain / 10000);
matthewgascoyne 10:d74124f266cb 465 s_ervo1 = s_ervo1 + newgain;
matthewgascoyne 10:d74124f266cb 466 //if(reading < discrpm){ s_ervo1 = s_ervo1 + gain; }
matthewgascoyne 10:d74124f266cb 467 //if(reading > discrpm){ s_ervo1 = s_ervo1 - gain; }
matthewgascoyne 10:d74124f266cb 468 if(reading == discrpm){ count++;
matthewgascoyne 10:d74124f266cb 469 if(count >= 10){ break; }
matthewgascoyne 10:d74124f266cb 470 }
matthewgascoyne 10:d74124f266cb 471 wait_ms(_RPM_CHANGE_DELAY_MS);
matthewgascoyne 10:d74124f266cb 472 }
matthewgascoyne 10:d74124f266cb 473 }
matthewgascoyne 10:d74124f266cb 474 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 475
matthewgascoyne 10:d74124f266cb 476 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 477 // "dshift_combine" : Function to combine all shift reg
matthewgascoyne 10:d74124f266cb 478 // elements into a single 16bit variable, for shifting.
matthewgascoyne 10:d74124f266cb 479 // This will return the 16bit variable.
matthewgascoyne 10:d74124f266cb 480 uint16_t dshift_combine(void) {
matthewgascoyne 10:d74124f266cb 481 uint16_t sdata_out = 0x0000; //clear the output shifter
matthewgascoyne 10:d74124f266cb 482 sdata_out |= (sdigi_a << 15); //now combine all elements
matthewgascoyne 10:d74124f266cb 483 sdata_out |= (sdigi_b << 14); //into the 16bits
matthewgascoyne 10:d74124f266cb 484 sdata_out |= (sdigi_c << 13); //
matthewgascoyne 10:d74124f266cb 485 sdata_out |= (sdigi_d << 12); //
matthewgascoyne 10:d74124f266cb 486 sdata_out |= (sdigi_e << 11); //
matthewgascoyne 10:d74124f266cb 487 sdata_out |= (sdigi_f << 10); //
matthewgascoyne 10:d74124f266cb 488 sdata_out |= (mux00_ch << 8); //
matthewgascoyne 10:d74124f266cb 489 sdata_out |= (mux01_ch << 6); //
matthewgascoyne 10:d74124f266cb 490 sdata_out |= (mux02_ch << 4); //
matthewgascoyne 10:d74124f266cb 491 sdata_out |= (mux03_ch << 2); //
matthewgascoyne 10:d74124f266cb 492 sdata_out |= mux04_ch; //
matthewgascoyne 10:d74124f266cb 493 return sdata_out;
matthewgascoyne 10:d74124f266cb 494 }
matthewgascoyne 10:d74124f266cb 495 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 496
matthewgascoyne 10:d74124f266cb 497 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 498 // "set_mux_chan" : Function to configure one of the five
matthewgascoyne 10:d74124f266cb 499 // daughter board multiplexers. (0 to 4). Each multiplexer
matthewgascoyne 10:d74124f266cb 500 // can select channels 1,2,3 or high-impedance (0).
matthewgascoyne 10:d74124f266cb 501 // This will effect the digital shift outputs and the
matthewgascoyne 10:d74124f266cb 502 // multiplexer configuration.
matthewgascoyne 10:d74124f266cb 503 void set_mux_chan(uint8_t mux, uint8_t chan) {
matthewgascoyne 10:d74124f266cb 504 switch(mux) {
matthewgascoyne 10:d74124f266cb 505 case 0: mux00_ch = chan; break;
matthewgascoyne 10:d74124f266cb 506 case 1: mux01_ch = chan; break;
matthewgascoyne 10:d74124f266cb 507 case 2: mux02_ch = chan; break;
matthewgascoyne 10:d74124f266cb 508 case 3: mux03_ch = chan; break;
matthewgascoyne 10:d74124f266cb 509 case 4: mux04_ch = chan; break;
matthewgascoyne 10:d74124f266cb 510 }
matthewgascoyne 10:d74124f266cb 511 uint16_t sdata_out = 0x0000; //clear the output shifter
matthewgascoyne 10:d74124f266cb 512 sdata_out = dshift_combine(); //combine into 16bits
matthewgascoyne 10:d74124f266cb 513 dshift_out(sdata_out); //perform the shift
matthewgascoyne 10:d74124f266cb 514 }
matthewgascoyne 10:d74124f266cb 515 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 516
matthewgascoyne 10:d74124f266cb 517 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 518 // "fdigi_out" : Function to assert digital outputs
matthewgascoyne 10:d74124f266cb 519 // attached directly to the mbed pins p29 and p30.
matthewgascoyne 10:d74124f266cb 520 void fdigi_out(uint8_t pin, bool pinval){
matthewgascoyne 10:d74124f266cb 521 switch(pin) {
matthewgascoyne 10:d74124f266cb 522 case 29: if(_p29_IO_Dir){p29_out = pinval;} break;
matthewgascoyne 10:d74124f266cb 523 case 30: if(_p30_IO_Dir){p30_out = pinval;} break;
matthewgascoyne 10:d74124f266cb 524 }
matthewgascoyne 10:d74124f266cb 525 }
matthewgascoyne 10:d74124f266cb 526 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 527
matthewgascoyne 10:d74124f266cb 528 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 529 // "sdigi_out" : Function to assert digital outputs
matthewgascoyne 10:d74124f266cb 530 // attached to the daughter board shift register.
matthewgascoyne 10:d74124f266cb 531 // digi channels a, b, c, d, e and f.
matthewgascoyne 10:d74124f266cb 532 // This will effect the digital shift outputs only.
matthewgascoyne 10:d74124f266cb 533 void sdigi_out(char digi_ch, bool pinval) {
matthewgascoyne 10:d74124f266cb 534 switch(digi_ch) {
matthewgascoyne 10:d74124f266cb 535 case 'a': sdigi_a = pinval; break;
matthewgascoyne 10:d74124f266cb 536 case 'b': sdigi_b = pinval; break;
matthewgascoyne 10:d74124f266cb 537 case 'c': sdigi_c = pinval; break;
matthewgascoyne 10:d74124f266cb 538 case 'd': sdigi_d = pinval; break;
matthewgascoyne 10:d74124f266cb 539 case 'e': sdigi_e = pinval; break;
matthewgascoyne 10:d74124f266cb 540 case 'f': sdigi_f = pinval; break;
matthewgascoyne 10:d74124f266cb 541 }
matthewgascoyne 10:d74124f266cb 542 uint16_t sdata_out = 0x0000; //clear the output shifter
matthewgascoyne 10:d74124f266cb 543 sdata_out = dshift_combine(); //combine into 16bits
matthewgascoyne 10:d74124f266cb 544 dshift_out(sdata_out); //perform the shift
matthewgascoyne 10:d74124f266cb 545 }
matthewgascoyne 10:d74124f266cb 546 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 547
matthewgascoyne 10:d74124f266cb 548
matthewgascoyne 10:d74124f266cb 549 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 550 // "rotate_scene_cont" : Function to turn ranging scene
matthewgascoyne 10:d74124f266cb 551 // to the left or the right by a small amount
matthewgascoyne 10:d74124f266cb 552 void rotate_scene_cont(uint8_t direction){
matthewgascoyne 10:d74124f266cb 553 while(disc1_in < 0.5f){} //wait for signal to go high
matthewgascoyne 10:d74124f266cb 554 wait_us(100); //short pause
matthewgascoyne 10:d74124f266cb 555 while(disc1_in > 0.5f){} //wait for signal to go low
matthewgascoyne 10:d74124f266cb 556 set_mux_chan(0,2); //allow rotate servo to move
matthewgascoyne 10:d74124f266cb 557 switch(direction){
matthewgascoyne 10:d74124f266cb 558 case 'l': {s_ervo3 = _def_servo3 + _ROTATE_SPEED; break;}
matthewgascoyne 10:d74124f266cb 559 case 'L': {s_ervo3 = _def_servo3 + _ROTATE_SPEED; break;}
matthewgascoyne 10:d74124f266cb 560 case 'r': {s_ervo3 = _def_servo3 - _ROTATE_SPEED; break;}
matthewgascoyne 10:d74124f266cb 561 case 'R': {s_ervo3 = _def_servo3 - _ROTATE_SPEED; break;} }
matthewgascoyne 10:d74124f266cb 562 wait_ms(_ROTATE_TIME_MS);
matthewgascoyne 10:d74124f266cb 563 s_ervo3 = _def_servo3;
matthewgascoyne 10:d74124f266cb 564 while(disc1_in < 0.5f){} //wait for signal to go high
matthewgascoyne 10:d74124f266cb 565 wait_us(100); //short pause
matthewgascoyne 10:d74124f266cb 566 while(disc1_in > 0.5f){} //wait for signal to go low
matthewgascoyne 10:d74124f266cb 567 set_mux_chan(0,3); //stop rotate servo from move
matthewgascoyne 10:d74124f266cb 568 }
matthewgascoyne 10:d74124f266cb 569 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 570
matthewgascoyne 10:d74124f266cb 571 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 572 // "rotate_scene" : Function to turn ranging scene
matthewgascoyne 10:d74124f266cb 573 // to the left or the right by a small amount
matthewgascoyne 10:d74124f266cb 574 void rotate_scene(uint8_t direction){
matthewgascoyne 10:d74124f266cb 575 switch(direction){
matthewgascoyne 10:d74124f266cb 576 case 'l': {s_ervo3 = s_ervo3 + _ROTATE_SPEED; break;}
matthewgascoyne 10:d74124f266cb 577 case 'L': {s_ervo3 = s_ervo3 + _ROTATE_SPEED; break;}
matthewgascoyne 10:d74124f266cb 578 case 'r': {s_ervo3 = s_ervo3 - _ROTATE_SPEED; break;}
matthewgascoyne 10:d74124f266cb 579 case 'R': {s_ervo3 = s_ervo3 - _ROTATE_SPEED; break;} }
matthewgascoyne 10:d74124f266cb 580 if(s_ervo3 < 0.0f){ s_ervo3 = 0.0f; }
matthewgascoyne 10:d74124f266cb 581 if(s_ervo3 > 1.0f){ s_ervo3 = 1.0f; }
matthewgascoyne 10:d74124f266cb 582 }
matthewgascoyne 10:d74124f266cb 583 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 584
matthewgascoyne 10:d74124f266cb 585 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 586 // "check_usb_rec_flg" : Function to see if any bytes
matthewgascoyne 10:d74124f266cb 587 // have come in, on the USB serial. Returns the
matthewgascoyne 10:d74124f266cb 588 // usb_receive_flg.
matthewgascoyne 10:d74124f266cb 589 bool check_usb_rec_flg(void){
matthewgascoyne 10:d74124f266cb 590 return usb_receive_flg;
matthewgascoyne 10:d74124f266cb 591 }
matthewgascoyne 10:d74124f266cb 592 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 593
matthewgascoyne 10:d74124f266cb 594 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 595 // "clr_usb_rec_flg" : Function to clear the
matthewgascoyne 10:d74124f266cb 596 // usb_receive_flg.
matthewgascoyne 10:d74124f266cb 597 void clr_usb_rec_flg(void){
matthewgascoyne 10:d74124f266cb 598 usb_receive_flg = false;
matthewgascoyne 10:d74124f266cb 599 }
matthewgascoyne 10:d74124f266cb 600 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 601
matthewgascoyne 10:d74124f266cb 602 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 603 // "usb_serial_r" : Function to deal with serial bytes
matthewgascoyne 10:d74124f266cb 604 // being received via USB serial. When a byte is rec'd
matthewgascoyne 10:d74124f266cb 605 // it is copied to a circular buffer and then a rec'd
matthewgascoyne 10:d74124f266cb 606 // flag is asserted to show that action is needed.
matthewgascoyne 10:d74124f266cb 607 void usb_serial_r (void) {
matthewgascoyne 10:d74124f266cb 608 //pc.printf("i'm in..\r\n");
matthewgascoyne 10:d74124f266cb 609 uint8_t character = pc.getc();
matthewgascoyne 10:d74124f266cb 610 if((usb_receive_pnt > (_USB_RECEIVE_BUF - 32)) &&
matthewgascoyne 10:d74124f266cb 611 (character == _HOST_MSG_START)){
matthewgascoyne 10:d74124f266cb 612 usb_receive_pnt = 0; //reset to start of buffer
matthewgascoyne 10:d74124f266cb 613 usb_receive_buf[usb_receive_pnt] = character; //save
matthewgascoyne 10:d74124f266cb 614 usb_receive_pnt++; //point to next empty buffer slot
matthewgascoyne 10:d74124f266cb 615 }
matthewgascoyne 10:d74124f266cb 616 else{
matthewgascoyne 10:d74124f266cb 617 usb_receive_buf[usb_receive_pnt] = character; //save
matthewgascoyne 10:d74124f266cb 618 usb_receive_pnt++; //point to next empty buffer slot
matthewgascoyne 10:d74124f266cb 619 }
matthewgascoyne 10:d74124f266cb 620 usb_receive_flg = true;
matthewgascoyne 10:d74124f266cb 621 //pc.putc(character);
matthewgascoyne 10:d74124f266cb 622 //pc.putc(character); pc.putc(usb_receive_buf[usb_receive_pnt-1]); //for testing
matthewgascoyne 10:d74124f266cb 623 //pc.printf("%d\r\n",usb_receive_pnt);
matthewgascoyne 10:d74124f266cb 624 //pc.printf("%d\r\n",usb_receive_flg);
matthewgascoyne 10:d74124f266cb 625 }
matthewgascoyne 10:d74124f266cb 626 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 627
matthewgascoyne 10:d74124f266cb 628 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 629 // "usb_serial_t" : Function to send array of characters
matthewgascoyne 10:d74124f266cb 630 // to the host PC via the usb serial. Returns the number
matthewgascoyne 10:d74124f266cb 631 // of characters written to the USB serial.
matthewgascoyne 10:d74124f266cb 632 uint8_t usb_serial_t (char txstring[]){
matthewgascoyne 10:d74124f266cb 633 uint8_t count = 0;
matthewgascoyne 10:d74124f266cb 634 while(txstring[count] != '\0'){
matthewgascoyne 10:d74124f266cb 635 while((!pc.writeable())){wait_us(_USB_TX_WAIT_US);}
matthewgascoyne 10:d74124f266cb 636 pc.putc(txstring[count]);
matthewgascoyne 10:d74124f266cb 637 count++;
matthewgascoyne 10:d74124f266cb 638 }
matthewgascoyne 10:d74124f266cb 639 //pc.printf("%d",count);
matthewgascoyne 10:d74124f266cb 640 return count;
matthewgascoyne 10:d74124f266cb 641 }
matthewgascoyne 10:d74124f266cb 642 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 643
matthewgascoyne 10:d74124f266cb 644 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 645 // "c_lookchk" : Function to check to see if the command
matthewgascoyne 10:d74124f266cb 646 // byte exists in any of the lookup tables. If it does
matthewgascoyne 10:d74124f266cb 647 // then true is returned.
matthewgascoyne 10:d74124f266cb 648 bool c_lookchk (uint8_t command){
matthewgascoyne 10:d74124f266cb 649 //pc.printf("command is %c\r\n",command);
matthewgascoyne 10:d74124f266cb 650 //pc.printf("c_matrix size is %d\r\n",sizeof(c_matrix));
matthewgascoyne 10:d74124f266cb 651 bool retval = false;
matthewgascoyne 10:d74124f266cb 652 for(uint8_t n=0;n<sizeof(c_matrix);n+=2){
matthewgascoyne 10:d74124f266cb 653 if(c_matrix[n] == command){
matthewgascoyne 10:d74124f266cb 654 retval = true;
matthewgascoyne 10:d74124f266cb 655 break;
matthewgascoyne 10:d74124f266cb 656 }
matthewgascoyne 10:d74124f266cb 657 }
matthewgascoyne 10:d74124f266cb 658 return retval;
matthewgascoyne 10:d74124f266cb 659 }
matthewgascoyne 10:d74124f266cb 660 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 661
matthewgascoyne 10:d74124f266cb 662 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 663 // "c_looklen" : Function to lookup the defined length of
matthewgascoyne 10:d74124f266cb 664 // the given command's data bytes. Returns the number of
matthewgascoyne 10:d74124f266cb 665 // data bytes that should exist. 0xFF denotes that the
matthewgascoyne 10:d74124f266cb 666 // command wasn't found in the lookup table.
matthewgascoyne 10:d74124f266cb 667 uint8_t c_looklen (uint8_t command){
matthewgascoyne 10:d74124f266cb 668 //pc.printf("command is %c\r\n",command);
matthewgascoyne 10:d74124f266cb 669 //pc.printf("c_matrix size is %d\r\n",sizeof(c_matrix));
matthewgascoyne 10:d74124f266cb 670 uint8_t retval = 0xFF;
matthewgascoyne 10:d74124f266cb 671 for(uint8_t n=0;n<sizeof(c_matrix);n+=2){
matthewgascoyne 10:d74124f266cb 672 if(c_matrix[n] == command){
matthewgascoyne 10:d74124f266cb 673 retval = c_matrix[n+1];
matthewgascoyne 10:d74124f266cb 674 break;
matthewgascoyne 10:d74124f266cb 675 }
matthewgascoyne 10:d74124f266cb 676 }
matthewgascoyne 10:d74124f266cb 677 return retval;
matthewgascoyne 10:d74124f266cb 678 }
matthewgascoyne 10:d74124f266cb 679 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 680
matthewgascoyne 10:d74124f266cb 681 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 682 // "check_msg" : That generates the final check byte from
matthewgascoyne 10:d74124f266cb 683 // the message found between tmp_pnt and chk_pnt. If the
matthewgascoyne 10:d74124f266cb 684 // check byte matches then true is returned.
matthewgascoyne 10:d74124f266cb 685 bool check_msg(uint8_t tmp_pnt, uint8_t chk_pnt){
matthewgascoyne 10:d74124f266cb 686
matthewgascoyne 10:d74124f266cb 687 //usb_receive_buf[0] = 'a';
matthewgascoyne 10:d74124f266cb 688 //usb_receive_buf[1] = 'b';
matthewgascoyne 10:d74124f266cb 689 //usb_receive_buf[2] = 'c';
matthewgascoyne 10:d74124f266cb 690 //usb_receive_buf[3] = '`';
matthewgascoyne 10:d74124f266cb 691
matthewgascoyne 10:d74124f266cb 692 bool retval = false;
matthewgascoyne 10:d74124f266cb 693 uint8_t tempxor = 0;
matthewgascoyne 10:d74124f266cb 694 for(uint8_t n=tmp_pnt;n<chk_pnt;n++){
matthewgascoyne 10:d74124f266cb 695 tempxor = tempxor ^ usb_receive_buf[n];
matthewgascoyne 10:d74124f266cb 696 //pc.printf("n is %d, tempxor is %c\r\n",n,tempxor);
matthewgascoyne 10:d74124f266cb 697 }
matthewgascoyne 10:d74124f266cb 698 if(tempxor == _HOST_MSG_START){ tempxor++; }
matthewgascoyne 10:d74124f266cb 699 if(tempxor == _MSG_END) { tempxor++; }
matthewgascoyne 10:d74124f266cb 700 if(tempxor == _MBED_MSG_START) { tempxor++; }
matthewgascoyne 10:d74124f266cb 701 //pc.printf("tempxor is %c\r\n",tempxor);
matthewgascoyne 10:d74124f266cb 702 //pc.printf("checkbyte is %c\r\n",usb_receive_buf[chk_pnt]);
matthewgascoyne 10:d74124f266cb 703 if(tempxor == usb_receive_buf[chk_pnt]){
matthewgascoyne 10:d74124f266cb 704 retval = true;
matthewgascoyne 10:d74124f266cb 705 }
matthewgascoyne 10:d74124f266cb 706 return retval;
matthewgascoyne 10:d74124f266cb 707 }
matthewgascoyne 10:d74124f266cb 708 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 709
matthewgascoyne 10:d74124f266cb 710 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 711 // "stan_reply" : Function to send a standard reply from
matthewgascoyne 10:d74124f266cb 712 // the mBed to the Host PC. A standard reply is one that
matthewgascoyne 10:d74124f266cb 713 // confirms that the message sent by the host has been
matthewgascoyne 10:d74124f266cb 714 // actioned.
matthewgascoyne 10:d74124f266cb 715 void stan_reply(char command){
matthewgascoyne 10:d74124f266cb 716 char msg[5] = {
matthewgascoyne 10:d74124f266cb 717 _MBED_MSG_START,
matthewgascoyne 10:d74124f266cb 718 command,
matthewgascoyne 10:d74124f266cb 719 _MSG_END, 0, '\0' };
matthewgascoyne 10:d74124f266cb 720 msg[3] = (_MBED_MSG_START ^ command) ^ _MSG_END;
matthewgascoyne 10:d74124f266cb 721 if(msg[3] == _HOST_MSG_START){ msg[3]=msg[3]+1; }
matthewgascoyne 10:d74124f266cb 722 if(msg[3] == _MSG_END) { msg[3]=msg[3]+1; }
matthewgascoyne 10:d74124f266cb 723 if(msg[3] == _MBED_MSG_START) { msg[3]=msg[3]+1; }
matthewgascoyne 10:d74124f266cb 724 usb_serial_t(msg);
matthewgascoyne 10:d74124f266cb 725 }
matthewgascoyne 10:d74124f266cb 726 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 727
matthewgascoyne 10:d74124f266cb 728 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 729 // "usb_serial_bufscan" : Function to check the INPUT
matthewgascoyne 10:d74124f266cb 730 // BUFFER to see if a valid command has been received. If
matthewgascoyne 10:d74124f266cb 731 // so then the function returns with the command ID.
matthewgascoyne 10:d74124f266cb 732 uint8_t usb_serial_bufscan (void){
matthewgascoyne 10:d74124f266cb 733 bool start_flag = false;
matthewgascoyne 10:d74124f266cb 734 uint8_t ret_cmd = 0;
matthewgascoyne 10:d74124f266cb 735 uint8_t temp_pnt = 0; uint8_t temp_end = 0;
matthewgascoyne 10:d74124f266cb 736 uint8_t temp_cmd = 0; uint8_t chk_pnt = 0;
matthewgascoyne 10:d74124f266cb 737 for(uint8_t n=0;n<_USB_RECEIVE_BUF;n++){
matthewgascoyne 10:d74124f266cb 738
matthewgascoyne 10:d74124f266cb 739 //pc.printf("n = [%d]\r\n",n);
matthewgascoyne 10:d74124f266cb 740
matthewgascoyne 10:d74124f266cb 741 if(usb_receive_buf[n] == _HOST_MSG_START){
matthewgascoyne 10:d74124f266cb 742 //pc.printf("'start' found @ [%d]\r\n",n);
matthewgascoyne 10:d74124f266cb 743 start_flag = true; //if start found then
matthewgascoyne 10:d74124f266cb 744 //pc.printf("start_flag = %s\r\n",start_flag ? "true" : "false");
matthewgascoyne 10:d74124f266cb 745 temp_pnt = n; //save the start pntr.
matthewgascoyne 10:d74124f266cb 746 //pc.printf("temp_pnt = [%d]\r\n",temp_pnt);
matthewgascoyne 10:d74124f266cb 747 }
matthewgascoyne 10:d74124f266cb 748 //else{ pc.printf("[%d] is not a start byte\r\n",n);}
matthewgascoyne 10:d74124f266cb 749
matthewgascoyne 10:d74124f266cb 750 if(((n-1) == temp_pnt) && (start_flag)){
matthewgascoyne 10:d74124f266cb 751 //pc.printf("Previous byte was start byte\r\n");
matthewgascoyne 10:d74124f266cb 752 //If startflag was set on previous iteration
matthewgascoyne 10:d74124f266cb 753 //Then lookup the command (current byte)
matthewgascoyne 10:d74124f266cb 754 start_flag = c_lookchk(usb_receive_buf[n]);
matthewgascoyne 10:d74124f266cb 755 if(start_flag){
matthewgascoyne 10:d74124f266cb 756 temp_cmd = usb_receive_buf[n];
matthewgascoyne 10:d74124f266cb 757 //pc.printf("..valid command is [%d]\r\n",n);
matthewgascoyne 10:d74124f266cb 758 }
matthewgascoyne 10:d74124f266cb 759 //else{ pc.printf("..but [%d] not a valid command\r\n",n);}
matthewgascoyne 10:d74124f266cb 760 //startflag will remain true if it's in any
matthewgascoyne 10:d74124f266cb 761 //of the lookup tables. Make a copy of the
matthewgascoyne 10:d74124f266cb 762 //valid command.
matthewgascoyne 10:d74124f266cb 763 }
matthewgascoyne 10:d74124f266cb 764 //else{ pc.printf("Previous byte wasnt start byte\r\n");}
matthewgascoyne 10:d74124f266cb 765
matthewgascoyne 10:d74124f266cb 766 if((usb_receive_buf[n] == _MSG_END) &&
matthewgascoyne 10:d74124f266cb 767 (start_flag)){
matthewgascoyne 10:d74124f266cb 768 //pc.printf("'end' found @ [%d]\r\n",n);
matthewgascoyne 10:d74124f266cb 769 //If startflag is still true and we find an
matthewgascoyne 10:d74124f266cb 770 //end character then check len(data bytes).
matthewgascoyne 10:d74124f266cb 771 temp_end = n;
matthewgascoyne 10:d74124f266cb 772 //pc.printf("temp_end = [%d]\r\n",n);
matthewgascoyne 10:d74124f266cb 773 //uint8_t j_f_test = c_looklen(temp_cmd);
matthewgascoyne 10:d74124f266cb 774 //pc.printf("should have %d data bytes\r\n",j_f_test);
matthewgascoyne 10:d74124f266cb 775 if(((temp_end - temp_pnt)-2) ==
matthewgascoyne 10:d74124f266cb 776 c_looklen(temp_cmd)) { chk_pnt = n+1;
matthewgascoyne 10:d74124f266cb 777 //pc.printf("..does have %d bytes\r\n",j_f_test);
matthewgascoyne 10:d74124f266cb 778 }
matthewgascoyne 10:d74124f266cb 779 //If the message is correct size we have to
matthewgascoyne 10:d74124f266cb 780 //check the next byte is a check byte.
matthewgascoyne 10:d74124f266cb 781 else { chk_pnt = 0;
matthewgascoyne 10:d74124f266cb 782 //j_f_test = ((temp_end - temp_pnt)-2);
matthewgascoyne 10:d74124f266cb 783 //pc.printf("..but instead has %d bytes\r\n",j_f_test);
matthewgascoyne 10:d74124f266cb 784 }
matthewgascoyne 10:d74124f266cb 785 }
matthewgascoyne 10:d74124f266cb 786 //else{ pc.printf("[%d] not an end byte\r\n",n);}
matthewgascoyne 10:d74124f266cb 787
matthewgascoyne 10:d74124f266cb 788 if((n == chk_pnt) && ((n-1) == temp_end)){
matthewgascoyne 10:d74124f266cb 789 //pc.printf("[%d] should be check byte\r\n",n);
matthewgascoyne 10:d74124f266cb 790 if(check_msg(temp_pnt, chk_pnt)){
matthewgascoyne 10:d74124f266cb 791 //pc.printf("..the check byte is valid");
matthewgascoyne 10:d74124f266cb 792 //save the data bytes somewhere
matthewgascoyne 10:d74124f266cb 793 //pc.printf("*need to save data bytes somewhere*\r\n");
matthewgascoyne 10:d74124f266cb 794 switch(temp_cmd) {
matthewgascoyne 10:d74124f266cb 795 case 'f': {
matthewgascoyne 10:d74124f266cb 796 c_data_0x66[0] = usb_receive_buf[n-4];
matthewgascoyne 10:d74124f266cb 797 c_data_0x66[1] = usb_receive_buf[n-3];
matthewgascoyne 10:d74124f266cb 798 c_data_0x66[2] = usb_receive_buf[n-2];
matthewgascoyne 10:d74124f266cb 799 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 800 if((c_data_0x66[0] == '2') && (c_data_0x66[1] == '9'))
matthewgascoyne 10:d74124f266cb 801 { fdigi_out(29,(c_data_0x66[2]-48)); }
matthewgascoyne 10:d74124f266cb 802 if((c_data_0x66[0] == '3') && (c_data_0x66[1] == '0'))
matthewgascoyne 10:d74124f266cb 803 { fdigi_out(30,(c_data_0x66[2]-48)); }
matthewgascoyne 10:d74124f266cb 804 //pc.printf("%c%c%c",_MBED_MSG_START,temp_cmd,_MSG_END);
matthewgascoyne 10:d74124f266cb 805 stan_reply(temp_cmd);
matthewgascoyne 10:d74124f266cb 806 break;
matthewgascoyne 10:d74124f266cb 807 }
matthewgascoyne 10:d74124f266cb 808 case 's': {
matthewgascoyne 10:d74124f266cb 809 c_data_0x73[0] = usb_receive_buf[n-3];
matthewgascoyne 10:d74124f266cb 810 c_data_0x73[1] = usb_receive_buf[n-2];
matthewgascoyne 10:d74124f266cb 811 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 812 sdigi_out(c_data_0x73[0], (c_data_0x73[1]-48));
matthewgascoyne 10:d74124f266cb 813 stan_reply(temp_cmd);
matthewgascoyne 10:d74124f266cb 814 break;
matthewgascoyne 10:d74124f266cb 815 }
matthewgascoyne 10:d74124f266cb 816 case 'i': {
matthewgascoyne 10:d74124f266cb 817 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 818 pc.printf("\r\n");
matthewgascoyne 10:d74124f266cb 819 boot_msg();
matthewgascoyne 10:d74124f266cb 820 stan_reply(temp_cmd);
matthewgascoyne 10:d74124f266cb 821 break;
matthewgascoyne 10:d74124f266cb 822 }
matthewgascoyne 10:d74124f266cb 823 case 'y': {
matthewgascoyne 10:d74124f266cb 824 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 825 int freq = directsync();
matthewgascoyne 10:d74124f266cb 826 set_discfreq(freq);
matthewgascoyne 10:d74124f266cb 827 freq_reply(freq, temp_cmd);
matthewgascoyne 10:d74124f266cb 828 break;
matthewgascoyne 10:d74124f266cb 829 }
matthewgascoyne 10:d74124f266cb 830 case 'p': {
matthewgascoyne 10:d74124f266cb 831 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 832 int dfreq = get_disc_freq();
matthewgascoyne 10:d74124f266cb 833 freq_reply(dfreq, temp_cmd);
matthewgascoyne 10:d74124f266cb 834 break;
matthewgascoyne 10:d74124f266cb 835 }
matthewgascoyne 10:d74124f266cb 836 case 'm': {
matthewgascoyne 10:d74124f266cb 837 c_data_0x6D[0] = (usb_receive_buf[n-3]-48);
matthewgascoyne 10:d74124f266cb 838 c_data_0x6D[1] = (usb_receive_buf[n-2]-48);
matthewgascoyne 10:d74124f266cb 839 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 840 set_mux_chan(c_data_0x6D[0],c_data_0x6D[1]);
matthewgascoyne 10:d74124f266cb 841 stan_reply(temp_cmd);
matthewgascoyne 10:d74124f266cb 842 break;
matthewgascoyne 10:d74124f266cb 843 }
matthewgascoyne 10:d74124f266cb 844 case 'd': {
matthewgascoyne 10:d74124f266cb 845 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 846 diag_reply();
matthewgascoyne 10:d74124f266cb 847 break;
matthewgascoyne 10:d74124f266cb 848 }
matthewgascoyne 10:d74124f266cb 849 case 'r': {
matthewgascoyne 10:d74124f266cb 850 c_data_0x72[0] = (usb_receive_buf[n-4]-48);
matthewgascoyne 10:d74124f266cb 851 c_data_0x72[1] = (usb_receive_buf[n-3]-48);
matthewgascoyne 10:d74124f266cb 852 c_data_0x72[2] = (usb_receive_buf[n-2]-48);
matthewgascoyne 10:d74124f266cb 853 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 854 float speedval = ((c_data_0x72[1]*10) + c_data_0x72[2]);
matthewgascoyne 10:d74124f266cb 855 speedval = speedval / 100;
matthewgascoyne 10:d74124f266cb 856 set_servo(c_data_0x72[0], speedval);
matthewgascoyne 10:d74124f266cb 857 stan_reply(temp_cmd);
matthewgascoyne 10:d74124f266cb 858 break;
matthewgascoyne 10:d74124f266cb 859 }
matthewgascoyne 10:d74124f266cb 860 case 'q': {
matthewgascoyne 10:d74124f266cb 861 c_data_0x71[0] = (usb_receive_buf[n-4]-48);
matthewgascoyne 10:d74124f266cb 862 c_data_0x71[1] = (usb_receive_buf[n-3]-48);
matthewgascoyne 10:d74124f266cb 863 c_data_0x71[2] = (usb_receive_buf[n-2]-48);
matthewgascoyne 10:d74124f266cb 864 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 865 int discrpm = (c_data_0x71[1]*10) + c_data_0x71[2];
matthewgascoyne 10:d74124f266cb 866 discrpm = discrpm + (c_data_0x71[0]*100);
matthewgascoyne 10:d74124f266cb 867 set_discfreq(discrpm);
matthewgascoyne 10:d74124f266cb 868 stan_reply(temp_cmd);
matthewgascoyne 10:d74124f266cb 869 break;
matthewgascoyne 10:d74124f266cb 870 }
matthewgascoyne 10:d74124f266cb 871 case 't': {
matthewgascoyne 10:d74124f266cb 872 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 873 stan_reply(temp_cmd);
matthewgascoyne 10:d74124f266cb 874 wait(5);
matthewgascoyne 10:d74124f266cb 875 mbed_reset();
matthewgascoyne 10:d74124f266cb 876 break;
matthewgascoyne 10:d74124f266cb 877 }
matthewgascoyne 10:d74124f266cb 878 case 'v': {
matthewgascoyne 10:d74124f266cb 879 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 880 volts_IR();
matthewgascoyne 10:d74124f266cb 881 break;
matthewgascoyne 10:d74124f266cb 882 }
matthewgascoyne 10:d74124f266cb 883 case 'o': {
matthewgascoyne 10:d74124f266cb 884 c_data_0x6F[0] = usb_receive_buf[n-2];
matthewgascoyne 10:d74124f266cb 885 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 886 rotate_scene_cont(c_data_0x6F[0]);
matthewgascoyne 10:d74124f266cb 887 stan_reply(temp_cmd);
matthewgascoyne 10:d74124f266cb 888 break;
matthewgascoyne 10:d74124f266cb 889 }
matthewgascoyne 10:d74124f266cb 890 case 'u': {
matthewgascoyne 10:d74124f266cb 891 command_clean(temp_pnt, chk_pnt);
matthewgascoyne 10:d74124f266cb 892 ultra_range();
matthewgascoyne 10:d74124f266cb 893 break;
matthewgascoyne 10:d74124f266cb 894 }
matthewgascoyne 10:d74124f266cb 895 }
matthewgascoyne 10:d74124f266cb 896 //pc.printf("'f' data bytes %c %c %c\r\n",c_data_0x66[0],c_data_0x66[1],c_data_0x66[2]);
matthewgascoyne 10:d74124f266cb 897 //pc.printf("'b' data bytes %c %c\r\n",c_data_0x62[0],c_data_0x62[1]);
matthewgascoyne 10:d74124f266cb 898 //pc.printf("'c' data bytes %c %c %c\r\n",c_data_0x63[0],c_data_0x63[1],c_data_0x63[2]);
matthewgascoyne 10:d74124f266cb 899 ret_cmd = temp_cmd;
matthewgascoyne 10:d74124f266cb 900 }
matthewgascoyne 10:d74124f266cb 901 }
matthewgascoyne 10:d74124f266cb 902 if(ret_cmd != 0){ break; }
matthewgascoyne 10:d74124f266cb 903 }
matthewgascoyne 10:d74124f266cb 904 //pc.printf("..returning ret_cmd\r\n");
matthewgascoyne 10:d74124f266cb 905 return ret_cmd;
matthewgascoyne 10:d74124f266cb 906 }
matthewgascoyne 10:d74124f266cb 907 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 908
matthewgascoyne 10:d74124f266cb 909 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 910 // "dio_setup" : Function to set up the standard digi IO
matthewgascoyne 10:d74124f266cb 911 void dio_setup (void){
matthewgascoyne 10:d74124f266cb 912 p29_out = _def_p29_out;
matthewgascoyne 10:d74124f266cb 913 p30_out = _def_p30_out;
matthewgascoyne 10:d74124f266cb 914 //disc0_out = _def_disc0;
matthewgascoyne 10:d74124f266cb 915 //disc1_out = _def_disc1;
matthewgascoyne 10:d74124f266cb 916 disc2_out = _def_disc2;
matthewgascoyne 10:d74124f266cb 917 //sync_out = _def_sync;
matthewgascoyne 10:d74124f266cb 918 }
matthewgascoyne 10:d74124f266cb 919 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 920
matthewgascoyne 10:d74124f266cb 921 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 922 // "servo_setup" : Function to set up the standard servos
matthewgascoyne 10:d74124f266cb 923 void servo_setup (void){
matthewgascoyne 10:d74124f266cb 924 s_ervo1 = _def_servo1;
matthewgascoyne 10:d74124f266cb 925 s_ervo2 = _def_servo2;
matthewgascoyne 10:d74124f266cb 926 s_ervo3 = _def_servo3;
matthewgascoyne 10:d74124f266cb 927 }
matthewgascoyne 10:d74124f266cb 928 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 929
matthewgascoyne 10:d74124f266cb 930 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 931 // "usb_serial_setup" : Function to set up the USB serial
matthewgascoyne 10:d74124f266cb 932 // interface.
matthewgascoyne 10:d74124f266cb 933 void usb_serial_setup (void){
matthewgascoyne 10:d74124f266cb 934 uint8_t n;
matthewgascoyne 10:d74124f266cb 935 for(n=0;n<_USB_RECEIVE_BUF;n++){ usb_receive_buf[n]=0x00;}
matthewgascoyne 10:d74124f266cb 936 usb_receive_pnt = 0; //clr buffer & setup pointer
matthewgascoyne 10:d74124f266cb 937 usb_receive_flg = false;//clr the byte rec'd flag
matthewgascoyne 10:d74124f266cb 938 pc.baud(_USB_BAUDRATE); //set the baudrate
matthewgascoyne 10:d74124f266cb 939 pc.attach(&usb_serial_r);; //configure the ISR
matthewgascoyne 10:d74124f266cb 940 }
matthewgascoyne 10:d74124f266cb 941 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 942
matthewgascoyne 10:d74124f266cb 943 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 944 // "scan_1s" : Function to check the recieve buffer
matthewgascoyne 10:d74124f266cb 945 // periodically (1s) to make sure there are no dormant
matthewgascoyne 10:d74124f266cb 946 // commands left in there.
matthewgascoyne 10:d74124f266cb 947 void scan_1s(void){
matthewgascoyne 10:d74124f266cb 948 led1 = 1; //heartbeat
matthewgascoyne 10:d74124f266cb 949 if(!check_usb_rec_flg()){
matthewgascoyne 10:d74124f266cb 950 uint8_t holder = usb_serial_bufscan();
matthewgascoyne 10:d74124f266cb 951 while( holder != 0 ){
matthewgascoyne 10:d74124f266cb 952 holder = usb_serial_bufscan();
matthewgascoyne 10:d74124f266cb 953 }
matthewgascoyne 10:d74124f266cb 954 }
matthewgascoyne 10:d74124f266cb 955 wait_us(10); led1 = 0; //heartbeat
matthewgascoyne 10:d74124f266cb 956 }
matthewgascoyne 10:d74124f266cb 957 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 958
matthewgascoyne 10:d74124f266cb 959 /////////////////////////////////////////////////////////
matthewgascoyne 10:d74124f266cb 960 // "startup_config" : Function to set up all the defaults
matthewgascoyne 10:d74124f266cb 961 // for the system.
matthewgascoyne 10:d74124f266cb 962 void startup_config (void){
matthewgascoyne 10:d74124f266cb 963 led1 = 0;
matthewgascoyne 10:d74124f266cb 964 usb_serial_setup(); //set up serial interface
matthewgascoyne 10:d74124f266cb 965 pc.printf(_BOOT_MESSAGE);
matthewgascoyne 10:d74124f266cb 966 pc.printf(" USBSERIAL:");
matthewgascoyne 10:d74124f266cb 967 pc.printf("%d:8:N:1\r\n",_USB_BAUDRATE);
matthewgascoyne 10:d74124f266cb 968 boot_msg(); //display the boot message
matthewgascoyne 10:d74124f266cb 969 dio_setup(); //set up standard digital IO
matthewgascoyne 10:d74124f266cb 970 servo_setup(); //set up standard servos
matthewgascoyne 10:d74124f266cb 971 uint16_t sdata_out = 0x0000; //clear the output shifter
matthewgascoyne 10:d74124f266cb 972 sdata_out = dshift_combine(); //combine into 16bits
matthewgascoyne 10:d74124f266cb 973 dshift_out(sdata_out); //perform the shift
matthewgascoyne 10:d74124f266cb 974 pc.printf(" FDIGIOUT");
matthewgascoyne 10:d74124f266cb 975 pc.printf(":%d",_def_p29_out);
matthewgascoyne 10:d74124f266cb 976 pc.printf(":%d",_def_p30_out);
matthewgascoyne 10:d74124f266cb 977 pc.printf("\r\n");
matthewgascoyne 10:d74124f266cb 978 pc.printf(" SDIGIOUT");
matthewgascoyne 10:d74124f266cb 979 pc.printf(":%d",_def_sdigioutA);
matthewgascoyne 10:d74124f266cb 980 pc.printf(":%d",_def_sdigioutB);
matthewgascoyne 10:d74124f266cb 981 pc.printf(":%d",_def_sdigioutC);
matthewgascoyne 10:d74124f266cb 982 pc.printf(":%d",_def_sdigioutD);
matthewgascoyne 10:d74124f266cb 983 pc.printf(":%d",_def_sdigioutE);
matthewgascoyne 10:d74124f266cb 984 pc.printf(":%d",_def_sdigioutF);
matthewgascoyne 10:d74124f266cb 985 pc.printf("\r\n");
matthewgascoyne 10:d74124f266cb 986 pc.printf(" MUXCONF");
matthewgascoyne 10:d74124f266cb 987 pc.printf(":%d",_def_mux00_ch);
matthewgascoyne 10:d74124f266cb 988 pc.printf(":%d",_def_mux01_ch);
matthewgascoyne 10:d74124f266cb 989 pc.printf(":%d",_def_mux02_ch);
matthewgascoyne 10:d74124f266cb 990 pc.printf(":%d",_def_mux03_ch);
matthewgascoyne 10:d74124f266cb 991 pc.printf(":%d",_def_mux04_ch);
matthewgascoyne 10:d74124f266cb 992 pc.printf("\r\n");
matthewgascoyne 10:d74124f266cb 993 pc.printf(" SERVO");
matthewgascoyne 10:d74124f266cb 994 pc.printf(":%d",(_def_servo1*100));
matthewgascoyne 10:d74124f266cb 995 pc.printf(":%d",(_def_servo2*100));
matthewgascoyne 10:d74124f266cb 996 pc.printf(":%d",(_def_servo3*100));
matthewgascoyne 10:d74124f266cb 997 pc.printf("\r\n");
matthewgascoyne 10:d74124f266cb 998 scan.attach(&scan_1s, 1.0); // setup "scan_1s" to run every 1.0s
matthewgascoyne 10:d74124f266cb 999 }
matthewgascoyne 10:d74124f266cb 1000 /////////////////////////////////////////////////////////