int main()
{
log_serial = new Serial(DEBUG_TX, DEBUG_RX);
log_serial->baud(9600);
DigitalIn sw2(SW2);
DigitalIn sw3(SW3);
for (int i = 0; i < SERVO_COUNT; i++) {
paint_heads[i] = PAINT_NEUTRAL;
}
radio_init(1500);
const uint64_t remote_addr64 = UINT64(0x0013A200, 0x40d4f162);
const XBeeLib::RemoteXBeeZB remoteDevice = XBeeLib::RemoteXBeeZB(
remote_addr64);
packet pkt;
const char *data = "what's up man?";
strncpy((char *) &pkt.data, data, 100);
pkt.len = strlen(data);
radio_send(pkt);
radio_bcast(pkt);
radio_send(pkt, remoteDevice);
uint8_t current_head = 0;
log_serial->printf("Hello!\r\n");
while (true) {
packet pkt;
if (radio_recv(pkt, 0)) {
printf(
"\r\nGot a %s RX packet [%08lx:%08lx|%04x], "
"len %d\r\nData: ",
pkt.broadcast ? "BROADCAST" : "UNICAST",
uint32_t (pkt.remote_addr64 >> 32),
uint32_t (pkt.remote_addr64 & 0xFFFFFFFF),
pkt.remote_addr16,
pkt.len);
printf("%.*s\r\n", pkt.len, pkt.data);
}
if (sw2 == 0) {
printf("painting\r\n");
while (sw2 == 0);
paint_heads[current_head % SERVO_COUNT] = PAINT_SPRAY;
wait(PAINT_TIME);
paint_heads[current_head % SERVO_COUNT] = PAINT_NEUTRAL;
}
if (sw3 == 0) {
printf("head %d\r\n", current_head);
current_head++;
while (sw3 == 0);
}
}
void task_application_imu_radio(uint16_t input) {
counter++;
P1OUT ^= 0x02; // toggle P1.1 for debug
P4OUT ^= 0x20; // toggle P4.5 for debug
P1OUT |= 0x04;P1OUT &= ~0x04; // pulse P1.2 for debug
if (counter==0) {
leds_circular_shift(); // circular shift LEDs for debug
}
//get RAM space for packet
testImuRadiopacketToSend = openqueue_getFreePacketBuffer();
//l1
testImuRadiopacketToSend->l1_channel = DEFAULTCHANNEL;
//payload
packetfunctions_reserveHeaderSize(testImuRadiopacketToSend,8);
gyro_get_measurement(&(testImuRadiopacketToSend->payload[0]));
P1OUT |= 0x04;P1OUT &= ~0x04; // pulse P1.2 for debug
packetfunctions_reserveHeaderSize(testImuRadiopacketToSend,6);
large_range_accel_get_measurement(&(testImuRadiopacketToSend->payload[0]));
P1OUT |= 0x04;P1OUT &= ~0x04; // pulse P1.2 for debug
packetfunctions_reserveHeaderSize(testImuRadiopacketToSend,6);
magnetometer_get_measurement(&(testImuRadiopacketToSend->payload[0]));
P1OUT |= 0x04;P1OUT &= ~0x04; // pulse P1.2 for debug
packetfunctions_reserveHeaderSize(testImuRadiopacketToSend,10);
sensitive_accel_temperature_get_measurement(&(testImuRadiopacketToSend->payload[0]));
P1OUT |= 0x04;P1OUT &= ~0x04; // pulse P1.2 for debug
packetfunctions_reserveFooterSize(testImuRadiopacketToSend,2);//space for radio to fill in CRC
//send packet
radio_send(testImuRadiopacketToSend);
}
void
inline sendPing()
{
pingPacket_t pingPacket = { PING, 1 };
radio_send(HOV2_ADDRESS, (uint8_t *)&pingPacket);
RADIO_SET_RECEIVE();
}
int main(void)
{
/* Setup radio input outputs */
radio_init();
/* Setup timer to 50hz (from timer1.c) */
/* Clear the timer counter */
TCNT1 = 0;
/* Prescaler will be FCPU/256 (Set bit CS02).
* So Timer freq will be 16000000/256 = 62500Hz
* We want 50Hz; 62500/50 = 1250. So we want an
* interrupt every 1250 timer1 ticks. */
OCR1A = 1250;
/* TIMSK: Enable Compare Match Interrupts (Set bit OCIE1A)*/
TIMSK |= _BV(OCIE1A);
/* TCCR1B: Clear timer on compare match (Set bit WGM12) */
TCCR1B |= _BV(WGM12);
/* TCCR1B: Prescaler to FCPU/256 & Enable (Set bit CS12) */
TCCR1B |= _BV(CS12);
/* Turn on interrupts */
sei();
/* Start the radio */
radio_send();
/* Sleep */
for (;;) sleep_mode();
}
static __inline void send_scan_rsp(const struct ll_pdu_adv *pdu)
{
struct ll_pdu_scan_req *scn;
/* Start replying as soon as possible, if there is something wrong,
* cancel it.
*/
radio_send((const uint8_t *) &pdu_scan_rsp, 0);
/* SCAN_REQ payload: ScanA(6 octets)|AdvA(6 octects) */
if (pdu->length != 12)
goto stop;
if (pdu->rx_add != laddr->type)
goto stop;
scn = (struct ll_pdu_scan_req *) pdu->payload;
if (memcmp(scn->adva, laddr->addr, 6))
goto stop;
return;
stop:
radio_stop();
}
/** Callback function for the "single shot" LL timer
*/
static void t_ll_single_shot_cb(void)
{
switch(current_state) {
case LL_STATE_ADVERTISING:
radio_stop();
radio_prepare(adv_chs[adv_ch_idx],
LL_ACCESS_ADDRESS_ADV, LL_CRCINIT_ADV);
radio_send((uint8_t *) &pdu_adv,
rx ? RADIO_FLAGS_RX_NEXT : 0);
prev_adv_ch_idx = adv_ch_idx;
if (!inc_adv_ch_idx())
timer_start(t_ll_single_shot,
t_adv_pdu_interval);
break;
case LL_STATE_SCANNING:
/* Called at the end of the scan window */
radio_stop();
break;
case LL_STATE_INITIATING:
case LL_STATE_CONNECTION:
/* Not implemented */
case LL_STATE_STANDBY:
default:
/* Nothing to do */
return;
}
}
void adv_interval_timeout(void *user_data)
{
radio_send(channels[idx++], ADV_CHANNEL_AA, ADV_CHANNEL_CRC, pdu,
sizeof(pdu));
if (idx < 3)
timer_start(adv_interval, ADV_INTERVAL, NULL);
}
void
inline reportToBase(uint8_t rightSpeed, uint8_t leftSpeed, uint8_t frontSonar,
uint8_t rightSonar, uint8_t leftSonar)
{
infoPacket_t infoPacket = { INFO, 0, frontSonar, rightSonar, leftSonar,
rightSpeed, leftSpeed };
radio_send(BASE_ADDRESS, (uint8_t *)&infoPacket);
RADIO_SET_RECEIVE();
}
void
inline sendMovements(uint8_t rightSpeed, uint8_t leftSpeed,
direction_t rightDir, direction_t leftDir)
{
movePacket_t movePacket = { MOVE, rightSpeed, leftSpeed,
rightDir, leftDir};
radio_send(HOV2_ADDRESS, (uint8_t *)&movePacket);
RADIO_SET_RECEIVE();
}
/*---------------------------------------------------------------------------*/
static int
transmit_packet(unsigned short len)
{
int ret = RADIO_TX_ERR;
if(pending_data != NULL) {
ret = radio_send(pending_data, len);
}
return ret;
}
void *th_advert( void *arg ){
while(1){
pthread_mutex_lock( &mutex_lock );
led1_on();
radio_send( BROADCAST_ADDR, RADIO_PORT, sizeof( my_info_t ), (rf_pkt*)&my_info );
led1_off();
pthread_mutex_unlock( &mutex_lock );
sleep( ADVERT_BASE_PERIOD );
}
}
//
// Timer tick handler
//
void application_timer_tick(timer *t)
{
if(canSend){
if(tx_buffer_inuse == false)
{
tx_buffer_inuse = true;
canSend = false;
memcpy(&tx_buffer, &newPkt, sizeof(Packet));
radio_send(tx_buffer, sizeof(Packet), newPkt.dst);
}
}
}
/*---------------------------------------------------------------------------*/
static int
radio_send(const void *payload, unsigned short payload_len)
{
int radiostate = simRadioHWOn;
if(!simRadioHWOn) {
/* Turn on radio temporarily */
simRadioHWOn = 1;
}
if(payload_len > COOJA_RADIO_BUFSIZE) {
return RADIO_TX_ERR;
}
if(payload_len == 0) {
return RADIO_TX_ERR;
}
if(simOutSize > 0) {
return RADIO_TX_ERR;
}
/* Copy packet data to temporary storage */
memcpy(simOutDataBuffer, payload, payload_len);
simOutSize = payload_len;
/* Transmit */
if (simOutSize > 0) {
PRINTF("COOJA: sending %d bytes @ %lu\n", simOutSize, clock_time());
}
while(simOutSize > 0) {
cooja_mt_yield();
}
simRadioHWOn = radiostate;
/* FIXME: this doesn't work */
cooja_mt_yield();
/* duplicate packets here */
if (clock_seconds() >= failure_delay[simMoteID]) {
if (!--packet_duplicate[simMoteID]) {
PRINTF("COOJA: sending %d bytes @ %lu (twice)\n", payload_len, clock_time());
radio_send(payload, payload_len);
}
}
return RADIO_TX_OK;
}
void isr_button() {
//prepare packet
testRadioPacketToSend = openqueue_getFreePacketBuffer();
//l1
testRadioPacketToSend->l1_channel = DEFAULTCHANNEL;
//payload
packetfunctions_reserveHeaderSize(testRadioPacketToSend,5);
testRadioPacketToSend->payload[0] = 0x01;
testRadioPacketToSend->payload[1] = 0x02;
testRadioPacketToSend->payload[2] = 0x03;
testRadioPacketToSend->payload[3] = 0x04;
testRadioPacketToSend->payload[4] = 0x05;
packetfunctions_reserveFooterSize(testRadioPacketToSend,2); //space for radio to fill in CRC
//send packet
radio_send(testRadioPacketToSend);
//debug
P1OUT ^= 0x02; // toggle P1.1 (for debug)
leds_circular_shift(); // circular-shift LEDs (for debug)
}
void mtsl_ranging(){
uint8_t i;
double meas_d;
for( i = 0; i < MAX_NB_CNT; i++ ){
if( i == LOCAL_ADDR ) continue;
if( radio_send( (uint16_t)i, 0xff, 0xff, (rf_pkt*)&meas_d ) < 0 ){
continue;
}
if( meas_d < 0 ) continue;
nb_past[i].d = nb_table[i].d;
nb_table[i].d = meas_d;
}
}
void task_application_intersection(uint16_t input) {
counter++;
P1OUT ^= 0x02; // toggle P1.1 for debug
P4OUT ^= 0x20; // toggle P4.5 for debug
P1OUT |= 0x04;P1OUT &= ~0x04; // pulse P1.2 for debug
if (counter==0) {
leds_circular_shift(); // circular shift LEDs for debug
}
//get RAM space for packet
testIntersection = openqueue_getFreePacketBuffer();
//l1
testIntersection->l1_channel = DEFAULTCHANNEL;
P1OUT ^= 0x02; // toggle P1.1 for debug
magnetometer_get_measurement(mag_reading);
mag_X = 0;
mag_Y = 0;
mag_Z = 0;
mag_X |= mag_reading[0]<<8;
mag_X |= mag_reading[1];
mag_Y |= mag_reading[2]<<8;
mag_Y |= mag_reading[3];
mag_Z |= mag_reading[4]<<8;
mag_Z |= mag_reading[5];
//note: in the following I use functions for simple multiplications
//and divisions for easy replacements in case the number of
//instruction cycles is too large to be acceptable in this application
mag_X = div_int(mag_X, 970);
mag_Y = div_int(mag_Y, 970);
mag_Z = div_int(mag_Z, 970);//970: look in HMC5843 datasheet
XX = mul_int(mag_X,mag_X);
YY = mul_int(mag_Y,mag_Y);
ZZ = mul_int(mag_Z,mag_Z);
mag_norm = XX + YY + ZZ; //no sqrt for faster execution
filt_norm = LPF(mag_norm);
//here we enter the state machine
switch (state) {
case NOCAR:
if (filt_norm>=threshold){
FSMcounter = 1;
state = PERHAPS;
}
break; //else you break
case PERHAPS:
if (filt_norm >= threshold && FSMcounter < maxCount){
FSMcounter++;
}
else if (filt_norm < threshold && FSMcounter >minCount)
FSMcounter--;
else if (filt_norm < threshold && FSMcounter <=minCount){
state = NOCAR;
FSMcounter=0;
if(seenCar){
seenCar=0;
packetfunctions_reserveHeaderSize(testIntersection,1);
testIntersection->payload[0] = seenCar;
packetfunctions_reserveFooterSize(testIntersection,2);//space for radio to fill in CRC
//send packet(noCar)
radio_send(testIntersection);
}
}
else if (filt_norm>=threshold && FSMcounter >=maxCount){
state=CAR;
if(!seenCar){
seenCar=1;
packetfunctions_reserveHeaderSize(testIntersection,1);
testIntersection->payload[0] = seenCar;
packetfunctions_reserveFooterSize(testIntersection,2);//space for radio to fill in CRC
//send packet(Car)
radio_send(testIntersection);
}
}
break;
case CAR:
if (filt_norm < threshold){
FSMcounter--;
state = PERHAPS;
}
break;
default:
break;
}
}
