static char getCh(UART* uart){ int val; do{ val = __uartGetByte(uart); }while(val==-1); char c = val; if(c>='a' && c<='z'){ c = c - 'a' + 'A'; } return c; }
static void _read(SENSOR* sensor){ int chIn = -1; char response[16]; uint8_t responseIndex; int16_t values[NUM_VALUES]; uint8_t fieldCount; RAZOR* razor = (RAZOR*)sensor; uint16_t timeout; // Send a '1' and await a response _uartSendByte(razor->uart, '1'); start: responseIndex = fieldCount = timeout = 0; while( chIn != ';'){ chIn = __uartGetByte(razor->uart); if(chIn == -1){ // No character available if(--timeout == 0){ // We have timed out - so keep values as before return; } }else{ // Get the character char c = chIn & 0xff; // Reset the timeout detector timeout = 0; // Ignore CR and LFs if(c == '\r' || c == '\n'){ goto start; } // Append character to the response if(responseIndex < sizeof(response)-1){ response[responseIndex++] = c; } // Check for end of field if(c ==',' || c==';'){ response[responseIndex] = 0; // Terminate the string int16_t value = atoi(response); // Convert number to an integer if(fieldCount < NUM_VALUES){ values[fieldCount] = value; // And store it if we have room } fieldCount ++; responseIndex = 0; // Reset for next field } } } if(fieldCount == NUM_VALUES ){ // We have a complete message with 9 values razor->imu.x_axis_degrees_per_second = values[0]; razor->imu.y_axis_degrees_per_second = values[1]; razor->imu.z_axis_degrees_per_second = values[2]; razor->imu.x_axis_mG = values[3]; razor->imu.y_axis_mG = values[4]; razor->imu.z_axis_mG = values[5]; razor->imu.bearingDegrees = values[6]; razor->imu.pitchDegrees = values[7]; razor->imu.rollDegrees = values[8]; } }
// Process incoming characters void gaitDesignerProcess(GAIT_DESIGNER* gait){ int b = __uartGetByte(gait->uart); if(b==-1) return; // No characters at all uint8_t inx = gait->msgInx; uint8_t* buffer = gait->buffer; Writer old = rprintfInit(gait->uart->writer); // Process all received characters while(b!=-1){ uint8_t servo; int8_t percent; uint8_t c = b & 0xff;; // _uartSendByte(gait->uart,c); if(c == '\r'){ // ignore it }else if(c=='\n'){ buffer[inx] = '\0'; // now process msg if(buffer[0]=='G' && inx>=3 && inx%2 ==1){ // A group message, inx=0; for(servo=0; servo < gait->num_actuators && buffer[inx+1];servo++){ inx++; int8_t percent = (hexDigit(buffer,inx) << 4); inx++; percent |= hexDigit(buffer,inx); setSpeed(gait,servo,percent); } }else if(buffer[0]=='N' && inx==1){ // Reply with number of servos rprintf("#n%d\r\n",gait->num_actuators); }else if(buffer[0]=='C'){ // Get config ie: C0 // Reply with: c0,center,range servo=0; inx=1; while(buffer[inx]>='0' && buffer[inx]<='9'){ servo*=10; servo+= (buffer[inx++] & 15); } SERVO* theServo = (SERVO*)getEntry(gait, servo); rprintf("#c%d,%d,%d\r\n",servo,theServo->center_us,theServo->range_us); }else if(buffer[0]=='S'){ // Single servo cmd <inx>,<speed> boolean neg = FALSE; servo=0; percent=0; inx=1; while(buffer[inx]>='0' && buffer[inx]<='9'){ servo*=10; servo+= (buffer[inx++] & 15); } if(buffer[inx++]==','){ if(buffer[inx]=='-'){ inx++; neg=TRUE; } while(buffer[inx]){ percent*=10; percent += (buffer[inx++] & 15); } if(neg){ percent *= -1; } setSpeed(gait,servo,percent); } } inx=0; }else if (c=='#'){ inx = 0; }else{ buffer[inx++] = b; } b = __uartGetByte(gait->uart); } gait->msgInx = inx; rprintfInit(old); }