void loop(){
//Send socket server data to motor controller
MasterSend();
//send motor controller data to socket server
MasterReceive();
}
void get_slave_status(void)
{
statusCode = 0;
success = 1;
// return;
slaveCmdBuff[0] = CMD_GET_STATUS;
(void) MasterTransmit( SLAVE_ADDRESS, slaveCmdBuff, 1 );
do {
success = MasterReceive( SLAVE_ADDRESS, &statusCode, 1 );
} while ((statusCode != 0) || (!success));
}
void read_from_slave(void)
{
statusCode = 0;
success = 1;
// return;
if (success)
{
do {
success = MasterReceive( SLAVE_ADDRESS, &statusCode, 1 );
} while ( !success );
}
}
void main(){
//int turn = 0;
//int rightspeed = 0;
//int leftspeed = 0;
setup();
while(1){
if(read(fd,&xbox,sizeof(xbox))<0){
}
else{
if(xbox.type==2 && xbox.number==5){
if(lmspeed>=0){
lmspeed = 0.003815*(xbox.value)+125;
}
else{
lmspeed = -(0.003815*(xbox.value)+125);
}
if(rmspeed>=0){
rmspeed = 0.003815*(xbox.value)+125;
}
else{
rmspeed = -(0.003815*(xbox.value)+125);
}
}
if(xbox.type==2 && xbox.number==2){
if(lmspeed>=0){
lmspeed = -(0.003815*(xbox.value)+125);
}
else{
lmspeed = (0.003815*(xbox.value)+125);
}
if(rmspeed>=0){
rmspeed = -(0.003815*(xbox.value)+125);
}
else{
rmspeed = (0.003815*(xbox.value)+125);
}
}
if(xbox.type==2 && xbox.number==0 && xbox.value<-10000){
//turn = xbox.value;
printf("Turning left\n");
float b = 1.878464*rmspeed;
float m = (8.7846e-5)*(float)rmspeed;
lmspeed = m*xbox.value+b;
}
if(xbox.type==2 && xbox.number==0 && xbox.value>4000){
printf("Turning right\n");
//turn = xbox.value;
float b = 1.278*lmspeed;
float m = -(6.95241e-5)*(float)lmspeed;
rmspeed = m*xbox.value+b;
}
printf("lmspeed = %i ",lmspeed);
printf("rmspeed = %i\n", rmspeed);
}
MasterSend(STARTBYTE,pwmf,lmspeed,lmbrake,rmspeed,rmbrake,sv[0],sv[1],sv[2],sv[3],sv[4],sv[5],devibrate,sensitivity,lowbat,i2caddr,i2cfreq);
//usleep(DELAY);
MasterReceive();
//usleep(DELAY);
}
}
int main(void){
unsigned int temp_int;
unsigned char val = 0;
CTS_DDR_REG |= (1<<CTS_PIN); // set to output
CTS_PORT_REG |= (1<<CTS_PIN); // set high
RTS_DDR_REG &= ~(1<<CTS_PIN); // set to input
RTS_PORT_REG |= (1<<CTS_PIN); // enable pull-up
mod_led_init();
InitTWI();
initbootuart(); // Initialize UART.
/* Main loop */
while(true)
{
val = recchar(); // Wait for command character.
switch(val) {
case 'P':
case 'L':
case 'E':
sendchar('\r');
break;
// Read lock byte -> execute command
case 'r':
switch(command_char) {
case 'a':
// NOT SUPPORTED in new code.
read_and_send( TWI_CMD_AVERSION );
break;
case 'b':
// NOT SUPPORTED in new code.
read_and_send( TWI_CMD_BVERSION );
break;
case 'd':
// Read CRCHI
sendchar(CRC_HI);
break;
case 'e':
// Read CRCLO
sendchar(CRC_LO);
break;
case 'f':
// Status condition
// NOT SUPPORTED in new code.
read_and_send(TWI_CMD_GETERRCONDN);
break;
default:
sendchar(0xFF);
break;
}
break;
case 'l':
// Write lock byte -> load command. NOT SUPPORTED in new code.
// NOTE: This looks like a hijacked command to do a CRC check.
command_char = recchar();
#if 0
if( command_char == 'c' )
{
send_command( TWI_CMD_CRCCHECK );
read_from_slave();
CRC_HI= statusCode;
read_from_slave();
CRC_LO = statusCode;
}
#endif
sendchar('\r');
break;
case 'N':
// Read high fuse bits -> BVERSION
read_and_send( TWI_CMD_BVERSION );
break;
case 'F':
// Low Fuse Bits -> AVERSION
read_and_send( TWI_CMD_AVERSION );
break;
case 'a':
sendchar('Y'); // Yes, we do auto-increment.
break;
case 'A':
addr =(recchar()<<8) | recchar(); // Read address high and low byte.
if(addr > MAX__APP_ADDR) over_size_flag = 1;
//+ 15mar17 ndp - send address to Slave.
slaveCmdBuff[0] = CMD_RECV_ADRS;
slaveCmdBuff[1] = (uint8_t)((addr>>8) & 0x00FF); // AH
slaveCmdBuff[2] = (uint8_t)(addr & 0x00FF); // AL
(void) MasterTransmit( SLAVE_ADDRESS, slaveCmdBuff, 3 );
//-
sendchar('\r'); // Send OK back.
break;
case 'e':
// Chip erase. NOT SUPPORTED in new code.
#if 0
runApp[0] = TWI_CMD_ERASEFLASH;
runApp[1] = TWI_CMD_ERASEFLASH;
get_slave_status();
success = MasterTransmit( SLAVE_ADDRESS, runApp, 2 );
#endif
sendchar('\r'); // Send OK back.
break;
case 'b':
// Check block load support.
sendchar('Y'); // Report block load supported.
sendchar((BLOCKSIZE>>8) & 0xFF); // MSB first.
sendchar(BLOCKSIZE&0xFF); // Report BLOCKSIZE (bytes).
over_size_flag = 0; // ndp 1-29-2017 fix
break;
case 'B':
// Start block load.
temp_int = (recchar()<<8) | recchar(); // Get block size.
val = recchar(); // Get memtype.
sendchar( BlockLoad(temp_int, val) ); // Block load.
// mod_led_toggle(4); // Need a short delay here.
pageBuffer[0] = CMD_RECV_DATA; // Address was sent in 'A' command service.
pageBuffer[1] = (uint8_t)(temp_int & 0x00FF); // NL..Only block size less than 256 supported.
// NOTE: Always sends PAGE_SIZE even if less data received from Host.
success = MasterTransmit( SLAVE_ADDRESS, pageBuffer, pageBuffer[1]+2 );
break;
case 'S':
// Return programmer identifier.
sendchar('A'); // Return 'AVRBOOT'.
sendchar('V'); // Software identifier (aka programmer signature) is always 7 characters.
sendchar('R');
sendchar('B');
sendchar('O');
sendchar('O');
sendchar('T');
reps =0;
break;
case 'V':
// Return software version.
// NOTE: TODO Should implement in new code.
// send_command(TWI_CMD_EXECUTEAPP);
// Disable bootloader mode for slave
sendchar('2');
sendchar('0');
break;
case 's':
// Return signature bytes [for the Target device ATtiny85].
slaveCmdBuff[0] = CMD_GET_SIG;
(void) MasterTransmit( SLAVE_ADDRESS, slaveCmdBuff, 1 );
mod_led_toggle(200); // Need a short delay here to let Slave set up data.
(void) MasterReceive( SLAVE_ADDRESS, slaveCmdBuff, 3 );
sendchar( slaveCmdBuff[2] );
sendchar( slaveCmdBuff[1] );
sendchar( slaveCmdBuff[0] );
break;
/* Add missing command .. ndp 01-29-2017
* Return Flash Data.
*
* TODO: Need to read from Slave.
*/
case 'g':
temp_int = (recchar()<<8) | recchar(); // Get block size.
val = recchar(); // Get mem type.
// NOTE: Address was sent in 'A' command process.
slaveCmdBuff[0] = CMD_GET_DATA;
slaveCmdBuff[1] = (uint8_t)(temp_int & 0x00FF);
(void) MasterTransmit( SLAVE_ADDRESS, slaveCmdBuff, 2 );
mod_led_toggle(200); // Need a short delay here to let Slave set up data.
(void) MasterReceive( SLAVE_ADDRESS, pageBuffer, (temp_int & 0x00FF) );
for(int i=0; i<temp_int; ++i)
{
sendchar( pageBuffer[i] );
}
break;
default:
if(val != 0x1b) { // If not ESC, then it is unrecognized...
sendchar('?');
}
break;
} // end: switch()
} // end: while(true)
} // end: main
