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);
}
