static void bitReceived(uint8_t bit) {
byteAccum <<= 1;
byteAccum |= bit;
bitCnt++;
if (bitCnt == 8) {
bitCnt = 0;
byteReceived(byteAccum);
}
}
void DevBase::process(){
if(m_port!=0){
char chByteArr[BUFFERINSZ]={0};
char *chByte=&chByteArr[0];
while(!m_bStop){
if(!m_bSend){
if(m_port->readPort(chByte,1)){
emit byteReceived(chByte);
}
}
usleep(LISTENERSLEEP);
}
}
emit finishedProcs();
}
void main()
{
//temp variable useful for loops
int i;
//temp variables for serial communication
int c;
unsigned int bytesRead;
//variables for wifi communication
int newMode;
int interfacePacketRecieved;
int network;
#GLOBAL_INIT
{
setMode(ROBOT_ENABLE);
rx.state = WAIT;
/***********************
Initialization of Packets
***********************/
// Initialize software version
aPacket[1] = SOFTWARE_VERSION_ID >> 8;
aPacket[2] = SOFTWARE_VERSION_ID & 0xFF;
// Initialize UDP response packets
gPacket[0] = 'G';
gPacket[1] = ROBOTMODE;
gPacket[2] = SOFTWARE_VERSION_ID >> 8;
gPacket[3] = SOFTWARE_VERSION_ID & 0xFF;
gPacket[4] = 0; // user version id
gPacket[5] = 0;
gPacket[6] = 0; // battery voltage
gPacket[7] = 0;
// Initialize J packet values to 0
memset(jPacket, 0, J_PACKET_LENGTH);
// Initialize X default packet (autonomous mode dummy packet) to 0
memset(xPacketDefaultValues, 0, X_PACKET_LENGTH);
}
/////////////////////////////////////////////////////////////////////////
// Configure Port D -- Leave PD4-PD7 untouched (used for other purposes)
/////////////////////////////////////////////////////////////////////////
WrPortI(PDCR, &PDCRShadow, RdPortI(PDCR) & 0xF0); // clear bits to pclk/2
WrPortI(PDFR, &PDFRShadow, RdPortI(PDFR) & 0xF0); // no special functions
WrPortI(PDDCR, &PDDCRShadow, RdPortI(PDDCR) & 0xF0); // clear bits to drive
// high and low
WrPortI(PDDR, &PDDRShadow, RdPortI(PDDR) | 0x0D); // set outputs high
WrPortI(PDDDR, &PDDDRShadow, RdPortI(PDDDR) | 0x0D); // set inputs and
// outputs
//Initialize serial communication
serialInit();
//Initialize controls as neutral
//memset(interface.axis, 127, sizeof(interface.axis));
//memset(interface.btn, 0, sizeof(interface.btn));
//Decide which router to connect to
if (BitRdPortI(PDDR, 1) == 1) {
//USER settings
BitWrPortI(PDDR, &PDDRShadow, 1, 0); // turn off user LED
network = USER_ROUTER;
printf("USER\n");
} else {
//COMPETITION Settings
BitWrPortI(PDDR, &PDDRShadow, 0, 0); // turn on user LED
network = COMP_ROUTER;
setMode(ROBOT_DISABLE);
printf("COMPETITION\n");
}
// */
printf("Robot Mode: %d \n", ROBOTMODE);
// Wait for X2 handshake (to learn team #) before attempting connection
printf("Waiting for X2...\n");
while (x2Connection != X2_CONN_ESTABLISHED) {
if ((c = serCgetc()) != -1) {
byteReceived(c);
}
}
printf("X2 connection established\n");
sendPacket('F', F_PACKET_LENGTH, fPacket);
ConnectToNetwork(network, teamNo);
//Main Loop
while(1) {
// Receive field, interface & X2 reply packets as they come
costate {
//Check to see if we have new field communication
if (udpRecieveFieldPacket()) {
newMode = ProcessFieldPacket();
if (newMode != -1) {
// Set robot mode flags
setMode(newMode);
// If disable flag set: zero motor values
if (newMode & ROBOT_DISABLE)
disableRobot();
// Send X2 packet with new mode flags
sendPacket('F', F_PACKET_LENGTH, fPacket);
// Send back response packet with our robot mode flags
// and code versions
sendFieldSocket(gPacket, G_PACKET_LENGTH);
//printf("Robot Mode: %d \n", ROBOTMODE);
}
}
//Check to see if we have new field commuication, and we can use it
if (udpRecieveInterfacePacket()) {
if (interBuf[0] == 'I') {
udpNewData = TRUE;
}
}
// Receive X2 serial data in bursts of up to X2_RX_BURST_LENGTH
bytesRead = 0;
while ((c = serCgetc()) != -1 &&
bytesRead <= X2_RX_BURST_LENGTH) {
byteReceived((unsigned char)c);
bytesRead += 1;
}
}
// */
// Time out if no UDP packets received after 500 ms
// TODO: Update the interface to send an enable packet. In the meantime
// this feature is only activated in competition mode
costate {
if (network == COMP_ROUTER) {
waitfor( udpNewData || DelayMs(500) );
if (!udpNewData)
disableRobot();
}
}
// Send X2 packets
costate {
// FOR TESTING
// udpNewData = TRUE;
waitfor( udpNewData );
// Check that disable bit is not set
if (!(ROBOTMODE & ROBOT_DISABLE)) {
// If in autonomous mode, send a dummy 'X' packet to keep the
// motor refreshing, but do not send actual joystick values
if (ROBOTMODE & ROBOT_AUTON)
sendPacket('X', X_PACKET_LENGTH, xPacketDefaultValues);
// Otherwise in enable mode, send the received joystick values
else
sendPacket('X', X_PACKET_LENGTH, interBuf+1);
x2OkToSend = FALSE;
udpNewData = FALSE;
// Wait for reply before sending another packet
waitfor( x2OkToSend || DelayMs(500) );
if (!x2OkToSend) { // no reply came within 500ms timeout
//printf("disable");
disableRobot();
}
}
}
costate {
// If auto bit is set, blink the LED fast
if (ROBOTMODE & ROBOT_AUTON) {
BitWrPortI(PDDR, &PDDRShadow, 0, 0); // turn on user LED
waitfor (DelayMs(100));
BitWrPortI(PDDR, &PDDRShadow, 1, 0); // turn off user LED
waitfor (DelayMs(100));
// Otherwise if disable bit is set, blink the LED slowly
} else if (ROBOTMODE & ROBOT_DISABLE) {
BitWrPortI(PDDR, &PDDRShadow, 0, 0); // turn on user LED
waitfor (DelayMs(500));
BitWrPortI(PDDR, &PDDRShadow, 1, 0); // turn off user LED
waitfor (DelayMs(500));
}
}
} // while(1)
} // main
