int main( void )
{
uint8_t already_converted = 0;
/* initialize Robostix 1 */
controller_init( );
while( 1 )
{
/* check if communication is upright */
if( flag_check_delay )
{
check_receive_delay( );
}
/* check if new serial or parallel data available */
if( serial_is_new_data( ) )//|| parallel_is_new_data( ) )
{
process_data_packet( );
}
/* check if new IMU data available */
if( dm3gx1_is_new_data( ) &&
!dm3gx1_get_data( &javiator_data ) )
{
javiator_data.state |= ST_NEW_DATA_IMU;
}
/* check if new analog sonar data available */
if( minia_is_new_data( ) && !already_converted )
{
/* convert sonar data */
adc_convert( ADC_CH_SONAR );
already_converted = 1;
}
else
if( !minia_is_new_data( ) )
{
already_converted = 0;
}
/* check if new converted sonar data available */
if( adc_is_new_data( ADC_CH_SONAR ) &&
!adc_get_data( ADC_CH_SONAR, &javiator_data.sonar ) )
{
javiator_data.state |= ST_NEW_DATA_SONAR;
}
/* check if new converted battery data available */
if( adc_is_new_data( ADC_CH_BATT ) )
{
adc_get_data( ADC_CH_BATT, &javiator_data.batt );
}
}
return( 0 );
}
/* Runs the control loop
*/
int main( void )
{
/* initialize Robostix 2 */
controller_init( );
while( 1 )
{
/* check if communication is upright */
if( flag_check_delay )
{
check_receive_delay( );
}
/* check if new parallel data available */
if( parallel_is_new_data( ) )
{
process_data_packet( );
}
/* check if new BMU data available */
if( bmu09a_is_new_data( ) &&
!bmu09a_get_data( &javiator_data ) )
{
javiator_data.state |= ST_NEW_DATA_BMU;
}
#if 0
/* check if new sonar data available */
if( adc_is_new_data( ADC_CH_SONAR ) &&
!adc_get_data( ADC_CH_SONAR, &javiator_data.sonar ) )
{
javiator_data.state |= ST_NEW_DATA_SONAR;
}
if( adc_is_new_data( ADC_CH_BATT ) )
{
adc_get_data( ADC_CH_BATT, &javiator_data.batt );
}
#endif
/* check if new x-laser data available */
if( lsm215_is_new_x_data( ) &&
!lsm215_get_x_data( javiator_data.x_pos ) )
{
javiator_data.state |= ST_NEW_DATA_POS_X;
LED_OFF( BLUE );
}
/* check if new y-laser data available */
if( lsm215_is_new_y_data( ) &&
!lsm215_get_y_data( javiator_data.y_pos ) )
{
javiator_data.state |= ST_NEW_DATA_POS_Y;
LED_ON( BLUE );
}
}
return( 0 );
}
/**
* @brief Measure the current consumption of a given spring.
* @return current consumption of a given spring (in microamps)
* warning 'spring' index starts at 1, not 0.
* @param[in] spring: selected spring ([1..SPRING_COUNT])
* @param[out] uA: selected spring current measurement, in microamps)
*/
int spwrm_get_current(uint8_t spring, int32_t *uA)
{
int ret;
uint8_t adc, chan;
uint32_t spin, uV;
CHECK_SPRING(spring);
CHECK_NULL_ARG(uA);
adc = spwrm_get_adc_device(spring);
chan = spwrm_get_adc_channel(spring);
spin = spwrm_get_sign_pin(spring);
*uA = 0;
ret = adc_get_data(adc, chan, &uV);
if (ret) {
dbg_error("%s(): failed to get %s data! (%d)\n", __func__,
spwrm_get_name(spring), ret);
return ret;
}
/* Convert to uV */
uV = adc_get_uV(uV);
/* Get data sign */
if (stm32_gpioread(spin) == 0) {
dbg_verbose("%s(): pin=0 => sign=-\n", __func__);
*uA = -((int32_t) max9929f_get_Iload(uV));
} else {
dbg_verbose("%s(): pin=1 => sign=+\n", __func__);
*uA = (int32_t) max9929f_get_Iload(uV);
}
dbg_verbose("%s(): measured voltage=%uuV => current=%duA\n",
__func__, uV, *uA);
return 0;
}
static hal_result_t s_cmd_get_ch_adc(uint8_t *data_in, uint8_t *data_out, uint8_t *len_data_out) //CAN_CMD_GET_CH_ADC: get ADC channel
{
hal_result_t res;
int16_t value;
int16_t ch_values[AN_CHANNEL_NUM];
calc_data_output_t tf_values;
if(data_in[1] >= AN_CHANNEL_NUM)
{
return(hal_res_NOK_wrongparam);
}
//***************************************************************************
//************************* IMPORTANTE **************************************
//==> FERMA L'ACQUISIZIONE DEI DATI se no si ha race condition!!!
//hal_timer_interrupt_disa(hal_timerT1);// stop timer to avoid rece condition on strain_cfg.ee_data.EE_AN_ChannelValue
//hal_timer_stop(hal_timerT1);
//in realtà qui non serve se uso i due buffer. Ovviamente non è l'ultimissimo valore letto
if(data_in[2] == 0) //raw value
{
res = adc_get_ch_value(data_in[1],0, &value);
}
else //calibrated data
{
res = adc_get_data(ch_values);
calculate_torque_and_force_data(ch_values, &tf_values);
value = tf_values.array[data_in[1]];
}
//***************************************************************************
//************************* IMPORTANTE **************************************
//==> FAI RIPARTIRE L'ACQUISIZIONE DEI DATI se no si ha race condition!!!
//hal_timer_start(hal_timerT1);
//hal_timer_interrupt_ena(hal_timerT1);
if(hal_res_OK != res)
{
return(hal_res_NOK_wrongparam);
}
value = value + 0x8000;
data_out[0] = CAN_CMD_GET_CH_ADC;
data_out[1] = data_in[1]; //channel number
data_out[2] = data_in[2]; //mode
data_out[3] = value >> 8;
data_out[4] = value & 0xFF;
*len_data_out = 5;
return(hal_res_OK);
}
int main(void)
{
int16_t len;
uint8_t * buf;
DynamixelPacket hostPacketIn;
DynamixelPacketInit(&hostPacketIn);
uint16_t adcVals[NUM_ADC_CHANNELS] = {1,2,3,4,5,6,7,8};
init();
/*
#ifdef DEBUG
//print out initialization message
DEBUG_COM_PORT_PUTSTR(MMC_READY_MSG);
#endif
*/
while(1)
{
//receive packet from host
len=HostReceivePacket(&hostPacketIn);
if (len>0)
HostPacketHandler(&hostPacketIn);
//receive a line from gps
len=GpsReceiveLine(&buf);
if (len>0)
GpsPacketHandler(buf,len);
//GpsPacketHandler(tempBuf,tempLen);
cli(); //disable interrupts to prevent race conditions while copying
len = adc_get_data(adcVals);
sei(); //re-enable interrupts
if (len > 0)
ImuPacketHandler(adcVals,len);
}
return 0;
}
uint16_t myip_stream_con_handler(uint8_t *in, uint16_t sz, uint8_t *out)
{
if(st.dir == 0)
return 0;
if(st.dir == STREAM_OUT)
{
#ifdef ENABLE_ADC
return adc_get_data(out, IO_BUF_SZ);
#endif
return 0;
}
if(st.dir == STREAM_IO)
{
#ifdef ENABLE_DSP
return dsp_io(in, sz, out);
#endif
return 0;
}
return 0;
}
extern void calc_current_tare_reset(void)
{
uint8_t i;
int16_t input[AN_CHANNEL_NUM];
int16_t output[AN_CHANNEL_NUM];
adc_get_data(input);
VectorAdd (AN_CHANNEL_NUM, (fractional*)input, (fractional*)&input[0], calc_data.SIXsg_config_ptr->SIXsg_ee_data.calibration_tare);
MatrixMultiply(
AN_CHANNEL_NUM, // int numRows1,
AN_CHANNEL_NUM, // int numCols1Rows2,
1, // int numCols2,
&output[0], // fractional* dstM,
&calc_data.SIXsg_config_ptr->SIXsg_ee_data.tr_matrix[0][0], // fractional* srcM1,
(int16_t*)&input[0]); // fractional* srcM2
for (i=0; i<AN_CHANNEL_NUM; i++)
{
calc_data.current_tare[i] = -(output[i]);
}
//nota: è corretto non aggiungere 0x8000 perchè è usato per la tara
}
int sci_adc_get_values(struct adc_sample_data *adc)
{
unsigned long flags, hw_flags;
int cnt = 12;
unsigned addr = 0;
unsigned val = 0;
int ret = 0;
int num = 0;
int sample_bits_msk = 0;
int *pbuf = 0;
if (!adc || adc->channel_id > ADC_MAX)
return -EINVAL;
pbuf = adc->pbuf;
if (!pbuf)
return -EINVAL;
num = adc->sample_num;
if (num > ADC_MAX_SAMPLE_NUM)
return -EINVAL;
sci_adc_lock();
sci_adc_config(adc); //configs adc sample.
addr = io_base + ADC_CTL;
val = adc_read(addr);
val &= ~(BIT_ADC_EN | BIT_SW_CH_ON | BIT_ADC_BIT_MODE_MASK);
adc_write(val, addr);
adc_clear_irq();
val = BIT_SW_CH_RUN_NUM(num);
val |= BIT_ADC_EN;
val |= BIT_ADC_BIT_MODE(adc->sample_bits);
val |= BIT_SW_CH_ON;
adc_write(val, addr);
while ((!adc_raw_irqstatus()) && cnt--) {
udelay(50);
}
if (!cnt) {
ret = -1;
WARN_ON(1);
goto Exit;
}
if (adc->sample_bits)
sample_bits_msk = ((1 << 12) - 1); //12
else
sample_bits_msk = ((1 << 10) - 1); //10
while (num--)
*pbuf++ = adc_get_data(sample_bits_msk);
Exit:
val = adc_read(addr);
val &= ~BIT_ADC_EN;
adc_write(val, addr);
sci_adc_unlock();
return ret;
}
static hal_result_t s_cmd_set_calib_tare(uint8_t *data_in, uint8_t *data_out, uint8_t *len_data_out) // CAN_CMD_SET_CALIB_TARE:
{
uint8_t i;
int16_t ch_values[AN_CHANNEL_NUM];
hal_result_t res = hal_res_OK;
int16_t sender_status;
adc_state_t adc_st;
sender_status = sender_is_running();
sender_stop();
switch(data_in[1])
{
case 0:
{
memset( CFG_6SG_EEDATA.calibration_tare, 0, (sizeof(uint16_t)*AN_CHANNEL_NUM) );
} break;
case 1:
{
adc_st =adc_is_running();
adc_stop();
adc_get_data(ch_values);
;
if(adc_st_started == adc_st)
{
adc_start();
}
for (i=0; i<AN_CHANNEL_NUM; i++)
{
CFG_6SG_EEDATA.calibration_tare[i]= -(ch_values[i]);
}
} break;
case 2:
{
if(data_in[2] >= AN_CHANNEL_NUM)
{
res= hal_res_NOK_wrongparam;
break;
}
CFG_6SG_EEDATA.calibration_tare[data_in[2]] = (data_in[3]<<8) | data_in[4];
} break;
default:
{
res= hal_res_NOK_wrongparam;
} break;
}
if(sender_status)
{
sender_start();
}
if(hal_res_OK != res)
{
parser_data.ee_data_is_saved = 0;
}
*len_data_out = 0;
return(res);
}
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;
}
