uint8 print_number2bytes(uint16 i){
uint8 cnt = 7;
uint8 j[] = {32,32,32,32,32,32,32,32};
if (i==0) { j[cnt] = 0 + 48;}
while (i>0){
j[cnt] = (i%10) + 48;
i = i/10;
cnt -= 1;
}
uart0TxSendByte(j[2]);
uart0TxSendByte(j[3]);
uart0TxSendByte(j[4]);
uart0TxSendByte(j[5]);
uart0TxSendByte(j[6]);
uart0TxSendByte(j[7]);
return cnt;
}
// Returns either CMDR_ALIVE_CNT or -1 depending on if a packet received or not
int8 CmdrReadMsgs(){
int8 buttonval;
while(uart1RxAvailable() > 0){
if(index_cmdr == -1){ // looking for new packet
if(uart1RxReceiveByte() == 0xff){ //read until packet start
index_cmdr = 0;
checksum_cmdr = 0;
// forward byte to other wixel
uart0TxSendByte(0xff);
}
}else if(index_cmdr == 0){
// add next byte to vals
vals[index_cmdr] = (unsigned char) uart1RxReceiveByte();
// forward byte to other wixel
uart0TxSendByte(vals[index_cmdr]);
// look for first real byte (non 0xFF)
if(vals[index_cmdr] != 0xff){
checksum_cmdr += (uint8) vals[index_cmdr];
index_cmdr++; // will now save subsequent bytes
}
}else{ //for bytes after the 0th byte
vals[index_cmdr] = (unsigned char) uart1RxReceiveByte();
// forward byte to other wixel
uart0TxSendByte(vals[index_cmdr]);
//loops will sequentially read bytes and store them here
checksum_cmdr += (uint8) vals[index_cmdr];
index_cmdr++;
// DEBUG: if all packets go through, shoudl see x2 through
// x7 when Commander input is being received.
// rprintf("x%u ",index_cmdr);
if(index_cmdr == 7){ // packet complete
// if(vals[0] == 0xff) {
// ax12LED(61,1);
// }
if(checksum_cmdr%256 != 255){
// packet error!
// rprintf("\npacket error!\n");
index_cmdr = -1;
return 0;
}
else{
buttonval = vals[4];
// short dowalking = TRUE;
//rprintf("\t%d\t",(int)buttonval);
//Turn gait...
// if((buttonval&0x40) > 0){ //if(buttonval & BUT_LT){
// // if(PRINT_DEBUG_COMMANDER){rprintf("lft\t");}
// turnleft = zTRUE;
// turnright = zFALSE;
// // dowalking = zFALSE;
// }
// else if((buttonval&0x80) > 0){ //if(buttonval & BUT_RT){
// // if(PRINT_DEBUG_COMMANDER){rprintf("rgt\t");}
// turnright = zTRUE;
// turnleft = zFALSE;
// // dowalking = zFALSE;
// }
// else { // Do nothing
// turnright = zFALSE;
// turnleft = zFALSE;
// // turn = zFALSE;
// }
if((buttonval&BUT_L6) > 0){
gunbutton = zTRUE;
// if(PRINT_DEBUG_COMMANDER){rprintf("guns\t");}
}
else{gunbutton = zFALSE;}
// if((buttonval&BUT_R3) > 0){
// panicbutton = zTRUE;
// // if(PRINT_DEBUG_COMMANDER){rprintf("panic\t");}
// }
// else{panicbutton = zFALSE;}
if((buttonval&BUT_L4) > 0){
laserbutton = zTRUE;
// if(PRINT_DEBUG_COMMANDER){rprintf("info\t");}
}
else{laserbutton = zFALSE;}
// if((buttonval&BUT_R2) > 0){
// // if(PRINT_DEBUG_COMMANDER){rprintf("look\t");}
// }
// else{infobutton = zFALSE;}
if((buttonval&BUT_R1) > 0){
solenoidbutton = zTRUE;
// if(PRINT_DEBUG_COMMANDER){rprintf("agit\t");}
}
else{solenoidbutton = zFALSE;}
// vals - 128 gives look a vlaue in the range from -128 to 127?
// lookV = (signed char)( (int8)vals[0]-128 );
// lookH = (signed char)( (int8)vals[1]-128 );
// if( (int)vals[0] >= 128){
// tilt_pos = interpolateU( (int)vals[0],128,128+102,TILT_CENTER,servo52Max);
// }
// else {
// tilt_pos = interpolateU( (int)vals[0],128-102,128,servo52Min,TILT_CENTER);
// }
//Default handling in original Commander.c - sets to range of -127 to 127 or so...
// walkV = (signed char)( (int8)vals[2]-128 );
// walkH = (signed char)( (int8)vals[3]-128 );
// }
// pan = (vals[0]<<8) + vals[1];
// tilt = (vals[2]<<8) + vals[3];
// buttons = vals[4];
// ext = vals[5];
}
index_cmdr = -1;
///Empty the packet buffer
while (uart1RxAvailable() > 0) { uart1RxReceiveByte(); }
return CMDR_ALIVE_CNT;
}
}
}
return -1;
} // End of CmdrReadMsgs
