static void led_rotation_rotate_leds_double(void) {
static uint16_t count;
count++;
led_all_off();
static const uint16_t count_max = 100;
if (count % count_max < count_max / 2) {
SET_LED(LED_1);
SET_LED(LED_3);
} else {
SET_LED(LED_2);
SET_LED(LED_4);
}
}
void standby(void)
{
battery_state = 0;
relai_state &= ~(RELAI_M_EN | RELAI_P_EN | RELAI_PD_EN);
RESET_RELAI(RELAIPLUS_CTRL);
RESET_RELAI(RELAIMINUS_CTRL);
_delay_ms(10);
RESET_RELAI(RELAICHARGE_CTRL);
#ifdef DCDCCTRL
RESET(DCDCCTRL);
#endif
RESET_LEDS();
SET_LED(LED_G);
if( charger_state )
{
charger_disable_output();
charger_disable_output();
charger_set_idle_curr();
charger_set_idle_curr();
charger_state = 0;
}
}
static void led_rotation_rotate_leds_race(void) {
static uint16_t count;
count++;
led_all_off();
static uint16_t count_max = 1000;
count_max--;
if (count_max == 20) {
count_max = 1000;
}
if (count % count_max < count_max / 4) {
SET_LED(LED_1);
} else if (count % count_max < count_max / 2) {
SET_LED(LED_2);
} else if (count % count_max < 3 * count_max / 4) {
SET_LED(LED_3);
} else {
SET_LED(LED_4);
}
}
void error(char *msg)
{
unsigned char i;
standby();
battery_state = STATE_ERROR;
for(i=0; i<8 ;i++)
{
error_msg[i] = msg[i];
}
RESET_LEDS();
SET_LED(LED_R);
}
void generate_pattern(char *dmx_universe)
{
int i;
static int count;
for (i = 0; i < 450; i++) {
int n = (unsigned char)dmx_universe[i];
n -= 2;
if (n < 0)
n = 0;
dmx_universe[i] = n;
}
if (count++ >= 10) {
int r;
count = 0;
r = rand();
r = r / (RAND_MAX/150);
if (r >= 150)
r = 149;
SET_LED(r, 255, 255, 255);
}
}
int main(void)
{
static unsigned int tmp = 0;
SET_OUTPUT(LED_R);
SET_OUTPUT(LED_G);
SET_OUTPUT(LED_B);
RESET_LED(LED_R);
RESET_LED(LED_G);
RESET_LED(LED_B);
#ifdef DCDCCTRL
SET_OUTPUT(DCDCCTRL);
RESET(DCDCCTRL);
#endif
battery_state = 0;
slave_init();
//uart_init();
timer_init();
mcan_init();
spi_init();
spi_adc_init();
relai_init();
adc_init();
sei();
if( (GPIOR0 & (1<<WDRF)) == (1<<WDRF) )
{
status("WDTRST ");
}
else if( (GPIOR0 & (1<<BORF)) == (1<<BORF) )
{
status("BORRST ");
}
else if( (GPIOR0 & (1<<EXTRF)) == (1<<EXTRF) )
{
status("START EX");
}
else if( (GPIOR0 & (1<<PORF)) == (1<<PORF) )
{
status("START PO");
}
else if( (GPIOR0 & (1<<JTRF)) == (1<<JTRF) )
{
status("START JT");
}
else
{
status("START ");
}
WDTCR = (1<<WDCE) | (1<<WDE);
WDTCR = (1<<WDE) | (1<<WDP2) | (1<<WDP1) | (1<<WDP0);
SET_LED(LED_G);
while (1)
{
__asm__ __volatile__ ("wdr");
if( battery_state & STATE_INTERMEDIATE )
{
intermediate_loop();
}
else if( battery_state & STATE_TRACTIVE )
{
tractive_loop();
}
else if( battery_state & STATE_CHARGING )
{
charge_loop();
}
else if( battery_state & STATE_BALANCING )
{
balancing_loop();
}
else
{
// standby mode
if( (tmp = mcan_check()) && !(battery_state & STATE_ERROR) )
{
switch(tmp)
{
case CAN_TRACTIVE_ENABLE: // CAN_TRACTIVE_ENABLE == 0x2F0
{
if( relai_volt < VCC_RELAI_MIN || vcc_volt < VCC_MIN )
{
status("NO VCC");
break;
}
// enable hv
intermediate_init();
dspace_heartbeat = 0;
battery_state |= STATE_TRACTIVE;
}
break;
case CAN_CHARGE_ENABLE: // CAN_CHARGE_ENABLE == 0x2FA
{
if( relai_volt < VCC_RELAI_MIN || vcc_volt < VCC_MIN )
{
status("NO VCC");
break;
}
// start charging
intermediate_init();
battery_state |= STATE_CHARGING;
}
break;
case CAN_BALANCING_ENABLE:
{
balancing_start();
battery_state |= STATE_BALANCING;
}
break;
}
}
}
slave_loop();
mcan_send_loop();
adc_loop();
}
return 0;
}
void generate_pattern(char *dmx_universe)
{
int i;
int bytes;
bytes = snd_rawmidi_read(midiin, &midi_buf, sizeof(midi_buf));
for (i = 0; i < bytes; i++) {
process_midi(midi_buf[i]);
}
for (i = 0; i < 150; i++) {
SET_LED(i, 0, 0, 0);
if (i < chan[0])
SET_LED(i, 255, 0, 0);
if (i < chan[1])
SET_LED(i, 0, 255, 0);
if (i < chan[2])
SET_LED(i, 0, 0, 255);
if (i < chan[3])
SET_LED(i, 255, 255, 0);
if (i < chan[4])
SET_LED(i, 255, 0, 255);
if (i < chan[5])
SET_LED(i, 0, 255, 255);
if (i < chan[6])
SET_LED(i, 255, 255, 255);
if (i < chan[7])
SET_LED(i, 0, 0, 0);
if ((i < chan[0x10]) && (i > (chan[0x10] - 10)))
SET_LED(i, 255, 0, 0);
if ((i < chan[0x11]) && (i > (chan[0x11] - 10)))
SET_LED(i, 0, 255, 0);
if ((i < chan[0x12]) && (i > (chan[0x12] - 10)))
SET_LED(i, 0, 0, 255);
if ((i < chan[0x13]) && (i > (chan[0x13] - 10)))
SET_LED(i, 255, 255, 0);
if ((i < chan[0x14]) && (i > (chan[0x14] - 10)))
SET_LED(i, 255, 0, 255);
if ((i < chan[0x15]) && (i > (chan[0x15] - 10)))
SET_LED(i, 0, 255, 255);
if ((i < chan[0x16]) && (i > (chan[0x16] - 10)))
SET_LED(i, 255, 255, 255);
if ((i < chan[0x17]) && (i > (chan[0x17] - 10)))
SET_LED(i, 0, 0, 0);
}
}
int emf_shootIntruder(int key)
{
static int status = 0;
unsigned int sw;
static BlobTracker *bt;
int blobfound;
double bx, by;
double error;
double resp;
CvPoint center;
if (key == 0)
{
status = 0;
bt = intruder;
is.hdPanAngle = 90;
send_command(SERVO_PAN, (byte)((int)is.hdPanAngle));
}
if (status == 0)
{
/*sendCommand(LTMTR_SDIR, 1, 0);
sendCommand(RTMTR_SDIR, 1, 0);
sendCommand(DIFF_DRV, is.ltMotorVal=150, is.rtMotorVal=165);*/
int i;
for (i=0; i<5; ++i)
{
while (1)
{
frame = cvQueryFrame(capture);
trackBlobs(bt, frame, 0);
blobfound = bt->blobs->trackable;
center = bt->blobs->center;
if (blobfound)
{
bx = ((double)center.x/(double)fsize.width)*100.0;
by = ((double)center.y/(double)fsize.height)*100.0;
printf("pos bx=%g, by=%g\n", bx, by);
error = bx-50.0;
printf("SHOOT PAN ERR= %g.\n", error);
if ((error>0&&error<10.0) || (error<0&&error>-10.0))
break;
//resp = getPidRespVerb(&driveMtrPID, error, fp, "DriveMtr");
resp = (double)error*0.2;
is.hdPanAngle += resp;
if (is.hdPanAngle < MAX_SERVO_PAN_LEFT)
is.hdPanAngle = MAX_SERVO_PAN_LEFT;
else
if (is.hdPanAngle > MAX_SERVO_PAN_RIGHT)
is.hdPanAngle = MAX_SERVO_PAN_RIGHT;
send_command(SERVO_PAN, (byte)((int)is.hdPanAngle));
}
else
{
status = 1;
return 1;
}
}
printf("in laser_shoot: %d.\n", i);
send_command(LASER_CMD, LASER_ON);
//sleep(2);
sw = getTickCount();
while ((getTickCount()-sw)<2000)
frame = cvQueryFrame(capture);
send_command(LASER_CMD, LASER_OFF);
sw = getTickCount();
while ((getTickCount()-sw)<2000)
frame = cvQueryFrame(capture);
trackBlobs(bt, frame, 0);
if (!bt->blobs->trackable)
break;
}
if (bt->blobs->trackable)
status = 2;
else
status = 1;
}
if (status == 1)
{
sw = getTickCount();
while ((getTickCount()-sw)<5000)
{
frame = cvQueryFrame(capture);
}
//sendCommand(BUZ_TOGGLE, 0, 0);
//++status;
removeState(&is, "intruder_near");
printf("Starting again.\n");
}
if (status == 2)
{
int status = 0;
//sendCommand(BUZ_TOGGLE, 0, 0);
send_command(ROBOT_STATUS, SET_LED(status, 3));
/*sw = getTickCount();
while ((getTickCount()-sw)<1000)
{
frame = cvQueryFrame(capture);
}
//sendCommand(BUZ_TOGGLE, 0, 0);
//++status;*/
removeState(&is, "intruder_near");
//removeState(&is, "gp_near");
addState(&is, "done_shooting");
printf("Finished\n");
}
return 1;
}
void led_all_on(void) {
for (uint8_t i = 0; i < LED_COUNT; i++) {
SET_LED(leds[i]);
}
}
/*******************************************************************************
* Function Name : radio_loop
* Description : When Radio is idle then checks for packets to be transmitted
* Input : None
* Output : None
* Return : None
*******************************************************************************/
void radio_loop()
{
// if the TX is idle then dequeue next packet and start TX
CLEAR_LED(RLED);
if ( 1 == stradio_retransmit_req_)
{
#if defined(_ENABLE_XBEE_COMPAT_4BS_TX_) || defined(_FORCE_XBEE_COMPAT_TX_)
#ifdef _FORCE_XBEE_COMPAT_TX_
if (1)
#else
if (BS_ADDR == txPacket[6])
#endif //_FORCE_XBEE_COMPAT_TX_
{
//txPacket[10]++;// = txPacket[3]; /* XBee COMPATIBILITY - increment sequence number */
txPacket[3]++;
}
#endif // defined(_ENABLE_XBEE_COMPAT_4BS_TX_) || defined(_FORCE_XBEE_COMPAT_TX_)
int temp_ret = ST_RadioTransmit(txPacket);
if (ST_SUCCESS != temp_ret)
{
txComplete = TRUE; // FAILED!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
stradio_retransmision_result = temp_ret;
}
else
{
txComplete = FALSE;
stradio_count_failed_retransmissions_++;
}
stradio_retransmit_req_ = 0;
} else
if (FALSE == txComplete)
{
SET_LED(RLED);
count_stalled++;
if (count_stalled > stalled_reset)
{
//stalled_reset = count_stalled + STALLED_THRESHOLD;
//txComplete = TRUE;
}
} else if ( 0 != stradio_pending_len_ )
{
sendPacketData( stradio_pending_len_, stradio_pending_data_, stradio_pending_dst_);
stradio_pending_len_ = 0;
} else if ( ( 0 == pkt_to_sent_len ) && ( 0xFF == pkt_to_sent_id ) )
{
stalled_reset++;
pkt_to_sent_id = que_deQpackets();
if ( 0xFF != pkt_to_sent_id )
{
char routed = 0;
unsigned int base = QBUFF_BASE ( pkt_to_sent_id );
// fill the TX-related variables
pkt_to_sent_len = PAK_GET_TOTAL_LENGTH ( pkt_to_sent_id );
// routing decisions for the packet
routed = routing_send_DATA_base ( QBUFF_BASE ( pkt_to_sent_id ) );
// If routing OK then start transmission process -> backoff
if ( 1 == routed )
{
unsigned int mac_d = get_dst_mac_base ( base );
sent_DATA_ = 1;
sendPacketData(pkt_to_sent_len, (sint8_t*)&(QBUFF_ACCESS(base, 0)) , mac_d ); // send via the backoff implementation
phy_sent_timeout_ = rtc_get_ticks() + my_tx_timeout_;
SET_LED(YLED);
}
else
{
if (ROUTING_BEGAN_ROUTE_DISCOVERY == routed)
{
// re-enqueue the packet
if (0 == que_enQpacket (pkt_to_sent_id))
{
release_pkt_in_tx();
}
else
{
pkt_to_sent_len = 0;
pkt_to_sent_id = 0xFF;
}
}
else
{
#ifdef _ENABLE_APP_MOD_
app_drop_pkt ( pkt_to_sent_id, MODULE_RTR, REASON_NOROUTE, EVENT_DSEND );
#endif // _ENABLE_APP_MOD_
// drop packet if not routable
release_pkt_in_tx();
}
}
}
}
}