int main(void)
{
int i;
int led = 0;
unsigned long count = 0;
// unsigned long baud = 57600;
unsigned long baud = 1000000;
int ret;
volatile float imuAngle[3];
// Variables to keep track for reading servos
int nRead=0, idRead=0;
uint8_t *ctableByte;
int serialWait = 0;
// Dynamixel packets and counters for input/output
DynamixelPacket *pktReadData, *pktStatus;
DynamixelPacket pktSerialInput, pktRs485Input;
init();
uart0_printf("\r\nStarting %s\r\n", CONTROLLER_NAME);
uart0_printf("Switching serial to %lu baud...\r\n", baud);
_delay_ms(100);
uart0_setbaud(baud);
uart0_printf("\r\nRunning serial at %lu baud\r\n", baud);
_delay_ms(100);
// Set Dynamixel return delay
rs485_putstrn("\xFF\xFF\xFE\x04\x03\x05\x00\xF5", 8);
while (1) {
count++;
if (count % 1000 == 0) {
// Toggle MON and Dynamixel Leds
if (led) {
led = 0;
sbi(PORTC, PORTC4);
rs485_putstrn("\xFF\xFF\xFE\x04\x03\x19\x00\xE1", 8);
}
else {
led = 1;
cbi(PORTC, PORTC4);
rs485_putstrn("\xFF\xFF\xFE\x04\x03\x19\x01\xE0", 8);
}
_delay_us(100);
}
if (uart0_recv(&pktSerialInput)) {
// RxD LED on
cbi(PORTC, PORTC1);
if (pktSerialInput.id == controlTable.id) {
switch (pktSerialInput.instruction) {
case INST_WRITE:
for (i = 1; i < pktSerialInput.length-2; i++) {
controlTablePtr[pktSerialInput.parameter[0]+i-1] =
pktSerialInput.parameter[i];
}
// Status packet
pktStatus = dynamixel_status(controlTable.id,
0, NULL, 0);
break;
case INST_READ:
pktStatus = dynamixel_status(controlTable.id, 0,
(uchar *)controlTablePtr+pktSerialInput.parameter[0],
pktSerialInput.parameter[1]);
break;
case INST_PING:
pktStatus = dynamixel_status(controlTable.id,
0, NULL, 0);
break;
default:
// Unknown command
pktStatus = dynamixel_status(controlTable.id,
1, NULL, 0);
break;
}
uart0_send(pktStatus);
}
else {
// Forward packet to RS485
rs485_send(&pktSerialInput);
if (pktSerialInput.id != DYNAMIXEL_BROADCAST_ID) {
serialWait = 1;
}
}
// RxD LED off
sbi(PORTC, PORTC1);
} // if (uart0_recv())
if (!serialWait) {
// TxD LED on
cbi(PORTC, PORTC2);
// Query servo for data in round robin fashion:
if (++nRead >= controlTable.nServo) nRead = 0;
idRead = controlTable.idMap[nRead];
pktReadData = dynamixel_instruction_read_data(idRead,
controlTable.addrRead,
controlTable.lenRead);
rs485_send(pktReadData);
// TxD LED off
sbi(PORTC, PORTC2);
} // if (!serialWait)
while (rs485_recv_timeout(&pktRs485Input, 300)) {
// Check if status packet contains requested read data
if (serialWait) {
// Forward packet to uart0
uart0_send(&pktRs485Input);
}
else if ((pktRs485Input.id == idRead) &&
(pktRs485Input.instruction == 0) &&
(pktRs485Input.length == controlTable.lenRead+2)) {
// Packet is correct return, flash EDIT Led
cbi(PORTC, PORTC3);
ctableByte = controlTable.dataRead + nRead*(controlTable.lenRead+1);
*ctableByte++ = idRead;
for (i=0; i<controlTable.lenRead; i++) {
*ctableByte++ = pktRs485Input.parameter[i];
}
sbi(PORTC, PORTC3);
}
} // while (rs485_recv())
// Timeout waiting for serial response
if (serialWait) {
if (++serialWait > 3) serialWait = 0;
}
//check to see if we got full set of adc values
//if the data is ready, it will be copied to the provided pointer
cli(); //disable interrupts to prevent race conditions while copying,
//since the interrupt-based ADC cycle will write asynchronously
ret = adc_get_data(controlTable.imuAcc);
sei(); //re-enable interrupts
/*
if (ret > 0)
ProcessImuReadings(controlTable.imuAcc,controlTable.imuAngle);
*/
/*
if (ret > 0) {
ProcessImuReadings(controlTable.imuAcc,controlTable.imuAngle);
for (i = 0; i<3;i++)
controlTable.imuAngle2[i]= 32768 + 1024* controlTable.imuAngle[i] ;
}*/
if (ret > 0) {
ProcessImuReadings(controlTable.imuAcc,imuAngle);
for (i = 0; i<3;i++)
controlTable.imuAngle[i]= 32768 + (uint16_t) 1024* imuAngle[i] ;
}
if (PINB & _BV(PB4)) {
//if pin high, the button is not pressed
controlTable.button = 0;
LED_ESTOP_PORT &= ~(_BV(LED_ESTOP_PIN));
}
else {
//if pin is low, the button is pressed
controlTable.button = 1;
LED_ESTOP_PORT |= _BV(LED_ESTOP_PIN);
}
} // while (1)
return 0;
}
int main(void)
{
int16_t len;
uint8_t * buf;
int c;
int imuRet;
int ret;
uint8_t ledState=0;
uint8_t * servo1PacketOut = NULL;
DynamixelPacket * servo1PacketIn = NULL;
uint8_t servo1PacketOutSize = 0;
float servo1Angle;
uint32_t servo1Time;
uint32_t newEstopTime;
estopState = MMC_ESTOP_STATE_RUN;
LED_ESTOP_ON;
estopPublishTimer = GlobalTimerGetTime();
estopTimeout = GlobalTimerGetTime();
DynamixelPacketInit(&hostPacketIn);
DynamixelPacketInit(&busPacketIn);
DynamixelPacketInit(&xbeePacketIn);
if (LoadAndSetEepromParams() != 0)
{
while(1)
{
//TODO: send the error packet
_delay_ms(100);
}
}
init();
//Servo1SetMode(SERVO_CONTROLLER_MODE_FB_ONLY);
//Servo1SetMode(SERVO_CONTROLLER_MODE_SERVO);
while(1)
{
//receive packet from host
len=HostReceivePacket(&hostPacketIn);
if (len>0)
HostPacketHandler(&hostPacketIn);
if (mode == MMC_MC_MODE_CONFIG)
continue;
//--------------------------------------------------------------------
// Estop Stuff
//--------------------------------------------------------------------
//check the state of the estop input
if (ESTOP_PORT & _BV(ESTOP_PIN)) //input high means disabled
{
estopState = MMC_ESTOP_STATE_DISABLE;
DisableVehicle();
}
//see if we need to send out estop status
newEstopTime = GlobalTimerGetTime();
if (newEstopTime > (estopPublishTimer+62500)) //1.0 seconds
{
estopPublishCntr++;
HostSendPacket(MMC_ESTOP_DEVICE_ID,MMC_ESTOP_STATE,
(uint8_t*)&estopState,1);
if ((estopPublishCntr % ESTOP_PUBLISH_MOD) == 0)
{
estopStateDataOut[0] = ptableR.id;
estopStateDataOut[1] = estopState;
XbeeSendPacket(MMC_ESTOP_DEVICE_ID,MMC_ESTOP_STATE,
estopStateDataOut,2);
}
estopPublishTimer = newEstopTime;
switch (estopState)
{
case MMC_ESTOP_STATE_RUN:
LED_ESTOP_ON;
LED_ERROR_OFF;
break;
case MMC_ESTOP_STATE_FREEZE:
LED_ESTOP_OFF;
LED_ERROR_TOGGLE;
break;
case MMC_ESTOP_STATE_DISABLE:
DisableVehicle();
break;
default:
break;
}
}
if (newEstopTime > (estopTimeout + 625000)) //10 seconds
{
estopState = MMC_ESTOP_STATE_FREEZE;
}
//--------------------------------------------------------------------
// RS-485 Bus
//--------------------------------------------------------------------
//receive packet from RS485 bus
servo1PacketIn = NULL;
len=BusReceivePacket(&busPacketIn);
if (len>0)
{
if (DynamixelPacketGetId(&busPacketIn) == MMC_DYNAMIXEL0_DEVICE_ID)
servo1PacketIn = &busPacketIn;
BusPacketHandler(&busPacketIn);
}
//--------------------------------------------------------------------
// Servo Controller
//--------------------------------------------------------------------
Servo1UpdateTime(GlobalTimerGetTime());
Servo1Update(servo1PacketIn,&servo1PacketOut,&servo1PacketOutSize);
if (servo1PacketOut && (servo1PacketOutSize > 0) && (estopState == MMC_ESTOP_STATE_RUN) )
{
memcpy(servo1PacketOutBuf,servo1PacketOut,servo1PacketOutSize);
servo1PacketOutBufSize = servo1PacketOutSize;
needToSendServo1Packet = 1;
}
//--------------------------------------------------------------------
// Xbee
//--------------------------------------------------------------------
len = XbeeReceivePacket(&xbeePacketIn);
if (len > 0)
XbeePacketHandler(&xbeePacketIn);
//--------------------------------------------------------------------
// GPS
//--------------------------------------------------------------------
//receive a line from gps
len=GpsReceiveLine(&buf);
if (len>0)
GpsPacketHandler(buf,len);
//--------------------------------------------------------------------
// ADC
//--------------------------------------------------------------------
cli();
len = adc_get_data(adcVals);
sei();
if (len > 0)
{
adcCntr++;
imuRet = ProcessImuReadings(adcVals,rpy,wrpy);
if (imuRet == 0) //will return 0 if updated, 1 if not yet updated
{
//send stuff out
memcpy(imuOutVals, &rpy[0], sizeof(float));
memcpy(imuOutVals+1,&rpy[1], sizeof(float));
memcpy(imuOutVals+2,&rpy[2], sizeof(float));
memcpy(imuOutVals+3,&wrpy[0],sizeof(float));
memcpy(imuOutVals+4,&wrpy[1],sizeof(float));
memcpy(imuOutVals+5,&wrpy[2],sizeof(float));
HostSendPacket(MMC_IMU_DEVICE_ID,MMC_IMU_ROT,
(uint8_t*)imuOutVals,6*sizeof(float));
}
if (imuRet == 1 || sendImuRaw == 1) //send out raw values if calibration is not finished
{
imuPacket[0] = adcCntr;
memcpy(&(imuPacket[1]),adcVals,NUM_ADC_CHANNELS*sizeof(uint16_t));
HostSendPacket(MMC_IMU_DEVICE_ID,MMC_IMU_RAW,
(uint8_t*)imuPacket,(NUM_ADC_CHANNELS+1)*sizeof(uint16_t));
}
if (estopState == MMC_ESTOP_STATE_RUN) //flicker leds when in run mode
{
if (adcCntr % 4 == 0)
DDRL = 0xFF;
else
DDRL = 0x00;
}
else
DDRL = 0xFF;
//switch colors when in run mode
if (adcCntr % 20 == 0)
{
ledState++;
if (ledState == 8)
ledState = 0;
if (estopState == MMC_ESTOP_STATE_RUN)
{
switch (ledState)
{
case 0:
PORTL = _BV(PL3);
break;
case 1:
PORTL = _BV(PL4);
break;
case 2:
PORTL = _BV(PL5);
break;
default:
PORTL = 0;
break;
}
}
else
PORTL = _BV(PL3); //paused
}
if (adcCntr % 100 == 0)
{
voltageBatt = adcVals[6]/1024.0*2.56*(11.0);
HostSendPacket(MMC_MAIN_CONTROLLER_DEVICE_ID,MMC_MC_VOLTAGE_BATT,
(uint8_t*)(&voltageBatt),sizeof(float));
if (voltageBatt < 21.0)
{
cli(); BUZZER_ON; sei();
}
else
{
cli(); BUZZER_OFF; sei();
}
}
}
//--------------------------------------------------------------------
// RS485 Bus Handling
//--------------------------------------------------------------------
if ( (needToSendServo1Packet == 1) && (rs485Blocked == 0))
{
SetBusBlocked();
BusSendRawData(servo1PacketOutBuf,servo1PacketOutBufSize);
needToSendServo1Packet = 0;
TCNT1 = 12500/4;
}
if ( (needToRequestFb == 1) && (rs485Blocked == 0) )
{
SetBusBlocked();
BusSendRawData(encoderRequestRawPacket,encoderRequestRawPacketSize);
needToRequestFb = 0;
}
}
return 0;
}
