void checkForInput()
{
static u08 var = 0;
static union InData tmp;
u08 c;
int in;
while( (in = uartGetByte()) != -1)
{
//uartSendByte(in);
c = in;
if(c == '#') // start of message
{
tmp.integer = 0;
var = 0;
}
else if(var == 0)
{
var = c;
}
else if(c >= '0' && c <= '9')
{
tmp.integer = (tmp.integer<<4) | (c - '0');
}
else if(c >= 'a' && c <= 'f')
{
tmp.integer = (tmp.integer<<4) | (c - 'a' + 10);
}
else if(c == '*') // end of message
{
if(var == 'C')
{
Thrust = tmp.bytes[3];
Yaw = (tmp.bytes[2] - 50);
Pitch = (tmp.bytes[1] - 50);
Roll = (tmp.bytes[0] - 50);
}
else if(var == 'P')
PID_Kp = (float)tmp.words[0] / 10.0f;
else if(var == 'I')
PID_Ki = (float)tmp.words[0] / 10.0f;
else if(var == 'D')
PID_Kd = (float)tmp.words[0] / 10.0f;
else if(var == 'Q')//P_yaw
PID_Kp_yaw = (float)tmp.words[0] / 10.0f;
else if(var == 'J')//I_yaw
PID_Ki_yaw = (float)tmp.words[0] / 10.0f;
else if(var == 'E')//D_yaw
PID_Kd_yaw = (float)tmp.words[0] / 10.0f;
else if(var == 'G')//D_yaw
Gain = (float)tmp.words[0] / 100.0f;
else if(var == 'M')
Thrust = tmp.bytes[0];
// setMotorSpeeds();
}
}
}
void serviceLocal(void)
{
int c;
// a little command-prompt utility to play with the spyglass UI
// all commands are single characters
if( (c = uartGetByte()) != -1)
{
// echo command to terminal
uartSendByte(c);
// process command
switch(c)
{
case 'i': spyglassLcdInit(); break;
case 'h':
rprintfInit(spyglassLcdWriteChar);
spyglassLcdGotoXY(0,0);
rprintf("*** HELLO WORLD ***");
rprintfInit(uartSendByte);
break;
case 'p':
rprintf("Pushbutton State: 0x%x\r\n", spyglassGetPushbuttons());
break;
case '+': if(Contrast<255) Contrast++; rprintf("\r\nLCD Contrast: %d\r\n", Contrast); spyglassSetLcdContrast(Contrast); break;
case '-': if(Contrast>0) Contrast--; rprintf("\r\nLCD Contrast: %d\r\n", Contrast); spyglassSetLcdContrast(Contrast); break;
case 'l': spyglassSetLeds(0x00); break;
case 'L': spyglassSetLeds(0xFF); break;
case 'b': spyglassSetBeeper(0); break;
case 'B': spyglassSetBeeper(1); break;
case 'x':
i2cDeviceSearch();
break;
case '?':
rprintfProgStrM("\r\n");
rprintfProgStrM("--- Spyglass Commands: ---\r\n");
rprintfProgStrM("(i) Initialize Spyglass LCD\r\n");
rprintfProgStrM("(h) Print 'Hello World' message to Spyglass LCD\r\n");
rprintfProgStrM("(p) Get Spyglass pushbutton state\r\n");
rprintfProgStrM("(+) Increase contrast number (lightens contrast)\r\n");
rprintfProgStrM("(-) Decrease contrast number (darkens contrast)\r\n");
rprintfProgStrM("(l) Set Spyglass User LEDs to OFF\r\n");
rprintfProgStrM("(L) Set Spyglass User LEDs to ON\r\n");
rprintfProgStrM("(b) Set Spyglass beeper to OFF\r\n");
rprintfProgStrM("(B) Set Spyglass beeper to ON\r\n");
rprintfProgStrM("--- General Commands: ---\r\n");
rprintfProgStrM("(x) Search for I2C devices on the bus\r\n");
rprintfProgStrM("(?) Help\r\n");
break;
case '\r':
default:
break;
}
// print new prompt
rprintfProgStrM("\r\ncmd>");
}
}
void mmcTest(void)
{
uint32_t sector=0;
uint8_t buffer[0x200];
int c;
// initialize MMC interface
mmcInit();
// print new prompt
rprintf("\r\ncmd>");
// testing loop
while(1)
{
// check for keypress
if((c=uartGetByte()) != -1)
{
switch(c)
{
case 'i':
// initialize card
rprintf("\r\nResetting MMC/SD Card\r\n");
mmcReset();
break;
case 'r':
// read current sector into buffer
rprintf("\r\nRead Sector %d\r\n", sector);
mmcRead(sector, buffer);
// print buffer contents
debugPrintHexTable(0x200, buffer);
break;
case 'w':
// write current sector with data from buffer
rprintf("\r\nWrite Sector %d\r\n", sector);
mmcWrite(sector, buffer);
break;
// move to new sector
case '+': sector++; rprintf("\r\nSector = %d", sector); break;
case '-': sector--; rprintf("\r\nSector = %d", sector); break;
case '*': sector+=512; rprintf("\r\nSector = %d", sector); break;
case '/': sector-=512; rprintf("\r\nSector = %d", sector); break;
case '\r':
default:
break;
}
// print new prompt
rprintf("\r\ncmd>");
}
}
}
void uartDataTask(void)
{
static unsigned char initStatus = 0;
if(!initStatus)
{
uartDataTaskInit();
initStatus = 1;
}
temp = uartGetByte();
switch(temp)
{
case 'R':
lineFollowerStart();
break;
case 'S':
lineFollowerStop();
break;
case 'W':
pToSettings = &lineFollowerPID.targetSpeed;
dtostrf(*pToSettings, 6, 0, tempString);
printf("Set target speed: %s", tempString);
break;
case 'P':
pToSettings = &lineFollowerPID.lineFollowerPidSettings.pGain;
dtostrf(*pToSettings, 6, 0, tempString);
printf("Set proportional: %s", tempString);
break;
case 'I':
pToSettings = &lineFollowerPID.lineFollowerPidSettings.iGain;
dtostrf(*pToSettings, 6, 0, tempString);
printf("Set integrative: %s", tempString);
break;
case 'D':
pToSettings = &lineFollowerPID.lineFollowerPidSettings.dGain;
dtostrf(*pToSettings, 6, 0, tempString);
printf("Set derivative: %s", tempString);
break;
case '+':
*pToSettings += 50;
dtostrf(*pToSettings, 6, 0, tempString);
printf("parameter = %s", tempString);
break;
case '-':
*pToSettings -= 50;
dtostrf(*pToSettings, 6, 0, tempString);
printf("parameter = %s", tempString);
break;
}
}
void tlvReceiveFSM(TLV_FSM *fsm, TLV_Buffer *tlvBuf, uint8 *ptr)
{
ptr[fsm->i] = uartGetByte();
switch(fsm->state)
{
case WAIT_FOR_TYPE:
if(ptr[fsm->i] == PROGRAMMING_MODE)
{
setProgrammingMode();
// while(Busy1USART()); //for debugging purpose
resetTarget = 1;
}
else if(ptr[fsm->i] == START_RUNNING)
{
setStartRunningMode();
resetTarget = 1;
}
else
fsm->state = WAIT_FOR_LENGTH;
break;
case WAIT_FOR_LENGTH:
fsm->length = ptr[fsm->i];
fsm->state = WAIT_FOR_VALUE;
break;
case WAIT_FOR_VALUE:
if((fsm->i - 1) < fsm->length)
fsm->state = WAIT_FOR_VALUE;
else
{
fsm->state = WAIT_FOR_TYPE;
if(!verifyCheckSum(ptr))
{
if(!verifyType(ptr))
uartSendByte(ERR_WRONG_TYPE);
else
uartSendByte(ERR_WRONG_CHECKSUM);
}
else
setTLVframe(tlvBuf, ptr);
}
break;
default:
break;
}
if(fsm->state == WAIT_FOR_TYPE)
fsm->i = 0;
else
fsm->i++;
}
void serviceLocal(void)
{
int c;
unsigned char buffer[100];
if ( (c = uartGetByte()) != -1)
{
// echo command to terminal
rprintfChar(c);
// process command
switch (c)
{
case 'i':
rprintfProgStrM("\r\nInitializing Ethernet Controller\r\n");
nicInit();
break;
case 'd':
rprintfProgStrM("\r\nEthernet Controller State\r\n");
nicRegDump();
break;
case 't':
rprintfProgStrM("\r\nCurrent Uptime: ");
rprintfNum(10, 9, FALSE, ' ', UptimeMs);
rprintfProgStrM("ms\r\n");
break;
case 'c':
rprintfProgStrM("\r\nCrashing System....\r\n");
while(1);
break;
case 'u':
rprintfProgStrM("\r\nSending UDP packet\r\n");
strcpy((char*)&buffer[ETH_HEADER_LEN+IP_HEADER_LEN+UDP_HEADER_LEN], "hello");
udpSend((ipGetConfig()->ip|~ipGetConfig()->netmask), CONTROL_PORT, 6, &buffer[ETH_HEADER_LEN+IP_HEADER_LEN+UDP_HEADER_LEN]);
break;
case '?':
rprintfProgStrM("\r\nCommands:\r\n");
rprintfProgStrM("(i) Initialize Ethernet Controller\r\n");
rprintfProgStrM("(d) Dump Ethernet Controller State\r\n");
rprintfProgStrM("(u) Send Broadcast UDP frame\r\n");
rprintfProgStrM("(t) Print current uptime\r\n");
rprintfProgStrM("(?) Help\r\n");
break;
case '\n':
default:
break;
}
// print new prompt
rprintfProgStrM("\r\ncmd>");
}
}
int uart3GetByte(void) {
return uartGetByte(3);
}
int uart2GetByte(void) {
return uartGetByte(2);
}
int uart1GetByte(void) {
return uartGetByte(1);
}
int uart0GetByte(void) {
return uartGetByte(0);
}
/* process messages coming from Commander
* format = 0xFF RIGHT_H RIGHT_V LEFT_H LEFT_V BUTTONS EXT checksum_cmdr */
int CmdrReadMsgs() {
//while(LISTEN.available() > 0){
while(uartReceiveBufferIsEmpty(LISTEN) == FALSE) {
if(index_cmdr == -1) { // looking for new packet
if(uartGetByte(LISTEN) == 0xff) { //read until packet start
index_cmdr = 0;
checksum_cmdr = 0;
}
} else if(index_cmdr == 0) {
vals[index_cmdr] = (unsigned char) uartGetByte(LISTEN);
if(vals[index_cmdr] != 0xff) {
checksum_cmdr += (int) vals[index_cmdr];
index_cmdr++;
}
} else {
vals[index_cmdr] = (unsigned char) uartGetByte(LISTEN); //loops will sequentially read bytes and store them here
checksum_cmdr += (int) 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(checksum_cmdr%256 != 255) {
// packet error!
rprintf("\npacket error!\n");
index_cmdr = -1;
return 0;
} else {
char buttonval = vals[4];
short dowalking = zTRUE;
//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) {
infobutton = zTRUE;
if(PRINT_DEBUG_COMMANDER) {
rprintf("info\t");
}
}
else {
infobutton = zFALSE;
}
if((buttonval&BUT_R2) > 0) {
pan_pos = PAN_CENTER;
tilt_pos = TILT_CENTER;
if(PRINT_DEBUG_COMMANDER) {
rprintf("look\t");
}
}
// else{infobutton = zFALSE;}
if((buttonval&BUT_R1) > 0) {
agitbutton = zTRUE;
if(PRINT_DEBUG_COMMANDER) {
rprintf("agit\t");
}
}
else {
agitbutton = zFALSE;
}
if((southpaw&0x01) > 0) { // SouthPaw
walkV = (signed char)( (int)vals[0]-128 );
walkH = (signed char)( (int)vals[1]-128 );
// lookV = (signed char)( (int)vals[2]-128 );
// lookH = (signed char)( (int)vals[3]-128 );
} else if (dowalking) {
// vals - 128 gives look a vlaue in the range from -128 to 127?
lookV = (signed char)( (int)vals[0]-128 );
lookH = (signed char)( (int)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);
// }
int pan_add = (-(float)lookH)/17;
int tilt_add = (-(float)lookV)/25;
pan_pos = CLAMP(pan_pos + pan_add, servo51Min, servo51Max);
tilt_pos = CLAMP(tilt_pos + tilt_add, servo52Min, servo52Max);
//Default handling in original Commander.c - sets to range of -127 to 127 or so...
walkV = (signed char)( (int)vals[2]-128 );
walkH = (signed char)( (int)vals[3]-128 );
}
// pan = (vals[0]<<8) + vals[1];
// tilt = (vals[2]<<8) + vals[3];
// buttons = vals[4];
// ext = vals[5];
}
index_cmdr = -1;
//LISTEN.flush(); //flush after reading an entire packet... why?
uartFlushReceiveBuffer(LISTEN);
return 1;
}
}
}
return 0;
}