void setServoAngle( uint8_t channel, uint8_t angle ){
uint8_t pulseLength = 40 + angle*200/180;
pulseLength = pulseLength*PWM_TIME_PERIOD_BASE/PWM_TIME_PERIOD;
updatePWM(channel, pulseLength);
startPWM();
timer_delay_ms(2000);
stopPWM();
timer_delay_ms(2000);
}
void pickup_car_from_cell(VerticalActuatorA* va, HorizontalActuatorK* ha) {
vertical_actuator_drop(va); // lower arm into 'dropped' position to reach under car
timer_delay_ms(DELAY_BETWEEN_MOVEMENTS_MS);
horizontal_actuator_extend(ha); // extend arm into cell to be underneath car
timer_delay_ms(DELAY_BETWEEN_MOVEMENTS_MS);
vertical_actuator_lift(va); // raise arm into 'normal' position, now supporting car
timer_delay_ms(DELAY_BETWEEN_MOVEMENTS_MS);
horizontal_actuator_retract(ha); // retract arm out of cell, now with car in tow
timer_delay_ms(DELAY_BETWEEN_MOVEMENTS_MS);
}
int32_t tui_gen_adjmenu(PGM_P header, printval_func_t *printer, int32_t min, int32_t max, int32_t start,
int32_t step)
{
tui_gen_menuheader(header);
uint8_t lbm = buttons_hw_count()==1?2:0;
int32_t idx=start;
/* No LBM for 16x2s... if i end up using them. */
if (LCD_MAXY<=2) lbm = 0;
for (;;) {
uint8_t buf[LCDWIDTH/2];
uint8_t sl = printer(buf, idx);
buf[sl] = 0;
sl = lcd_strwidth(buf);
lcd_gotoxy_dw(0,1);
lcd_clear_dw((LCDWIDTH - sl)/2);
lcd_puts_dw(buf);
lcd_clear_eol();
if (lbm) {
lcd_gotoxy_dw(0,2);
lcd_puts_dw_P(lbm==2?PSTR("DIR: NEXT"):PSTR("DIR: PREV"));
lcd_clear_eol();
}
lcd_clear_eos();
if (lbm) {
timer_delay_ms(180);
timer_delay_ms(100);
}
uint8_t key = tui_waitforkey();
if ((lbm==1)&&(key&BUTTON_NEXT)) key = BUTTON_PREV;
switch (key) {
case BUTTON_NEXT:
if ((idx+step) > max) {
idx = min;
} else {
idx += step;
}
break;
case BUTTON_PREV:
if ((idx-step) < min) {
idx = max;
} else {
idx -= step;
}
break;
case BUTTON_OK:
if (lbm==2) {
lbm = 1;
break;
}
return idx;
case BUTTON_CANCEL:
return start;
}
}
}
void dropoff_car_in_cell(VerticalActuatorA* va, HorizontalActuatorK* ha) {
vertical_actuator_lift(va); // ensure arm is in 'normal' position
timer_delay_ms(DELAY_BETWEEN_MOVEMENTS_MS);
horizontal_actuator_extend(ha); // extend into cell
timer_delay_ms(DELAY_BETWEEN_MOVEMENTS_MS);
vertical_actuator_drop(va); // send arm into 'dropped' position, no longer supporting car
timer_delay_ms(DELAY_BETWEEN_MOVEMENTS_MS);
horizontal_actuator_retract(ha); // retract out of cell
timer_delay_ms(DELAY_BETWEEN_MOVEMENTS_MS);
vertical_actuator_lift(va); // bring arm into 'normal' position
timer_delay_ms(DELAY_BETWEEN_MOVEMENTS_MS);
}
int wrc_ptp_set_mode(int mode)
{
uint32_t start_tics, lock_timeout = 0;
ptp_mode = 0;
wrc_ptp_stop();
switch (mode) {
case WRC_MODE_GM:
rtOpts.primarySource = TRUE;
rtOpts.wrConfig = WR_M_ONLY;
rtOpts.masterOnly = TRUE;
spll_init(SPLL_MODE_GRAND_MASTER, 0, 1);
lock_timeout = LOCK_TIMEOUT_GM;
break;
case WRC_MODE_MASTER:
rtOpts.primarySource = FALSE;
rtOpts.wrConfig = WR_M_ONLY;
rtOpts.masterOnly = TRUE;
spll_init(SPLL_MODE_FREE_RUNNING_MASTER, 0, 1);
lock_timeout = LOCK_TIMEOUT_FM;
break;
case WRC_MODE_SLAVE:
rtOpts.primarySource = FALSE;
rtOpts.wrConfig = WR_S_ONLY;
rtOpts.masterOnly = FALSE;
spll_init(SPLL_MODE_SLAVE, 0, 1);
break;
}
initDataClock(&rtOpts, &ptpClockDS);
start_tics = timer_get_tics();
mprintf("Locking PLL");
shw_pps_gen_enable_output(0);
while (!spll_check_lock(0) && lock_timeout) {
timer_delay_ms(1000);
mprintf(".");
if (time_after(timer_get_tics(), start_tics + lock_timeout)) {
mprintf("\nLock timeout.\n");
return -ETIMEDOUT;
} else if (uart_read_byte() == 27) {
mprintf("\n");
return -EINTR;
}
}
mprintf("\n");
if (mode == WRC_MODE_MASTER || mode == WRC_MODE_GM)
shw_pps_gen_enable_output(1);
ptp_mode = mode;
return 0;
}
int32_t tui_gen_menupart(unsigned char* line, unsigned char* buf, printval_func_t *printer, int32_t min, int32_t max, int32_t start, int32_t step, uint8_t delay) {
int32_t idx=start;
uint8_t z,y;
for (;;) {
uint8_t key;
y = printer(buf,idx);
memset(line,' ',16);
z = (16 - y) / 2;
memcpy(&(line[z]),buf,y);
lcd_gotoxy(0,1);
lcd_puts(line);
if (delay) timer_delay_ms(delay);
key = tui_waitforkey();
switch (key) {
case BUTTON_S1:
if ((idx+step) > max) idx = min;
else idx += step;
break;
case BUTTON_S2:
if ((idx-step) < min) idx = max;
else idx -= step;
break;
case BUTTON_BOTH:
return idx;
}
}
}
/* legacy function for 'calibration force' command */
int measure_t24p(uint32_t *value)
{
int rv;
pp_printf("Waiting for link...\n");
while (!ep_link_up(NULL))
timer_delay_ms(100);
spll_init(SPLL_MODE_SLAVE, 0, 1);
pp_printf("Locking PLL...\n");
while (!spll_check_lock(0))
timer_delay_ms(100);
pp_printf("\n");
pp_printf("Calibrating RX timestamper...\n");
rxts_calibration_start();
while (!(rv = rxts_calibration_update(value))) ;
return rv;
}
int tui_enh_listmenu(PGM_P header, listprint_func_t *printer, uint8_t entries, uint8_t start)
{
tui_gen_menuheader(header);
uint8_t idx=start;
uint8_t scroll=0;
PGM_P idstr = PSTR("> ");
uint8_t idw = lcd_strwidth_P(idstr);
const uint8_t brackl = entries < (LCD_MAXY-1) ? entries : LCD_MAXY - 1;
for (;;) {
int8_t vbp = idx-scroll;
if (vbp < 0) {
scroll += vbp;
} else if (vbp>=brackl) {
scroll += (vbp-brackl)+1;
}
vbp = idx-scroll;
if ((!vbp)&&(idx)) {
scroll--;
} else if ((vbp == brackl-1)&&(idx<(entries-1))) {
scroll++;
}
vbp = idx-scroll;
uint8_t vi = idx - vbp;
for (uint8_t bp=0; bp<brackl; bp++) {
lcd_gotoxy_dw(0, bp+1);
if (bp==vbp) lcd_puts_dw_P(idstr);
else lcd_clear_dw(idw);
printer(vi);
vi++;
}
lcd_clear_eos();
timer_delay_ms(50);
uint8_t key = tui_waitforkey();
switch (key) {
case BUTTON_NEXT:
if ((idx+1) >= entries) {
idx = 0;
} else {
idx++;
}
break;
case BUTTON_PREV:
if (!idx) {
idx = entries-1;
} else {
idx--;
}
break;
case BUTTON_OK:
return idx;
case BUTTON_CANCEL:
return -1;
}
}
}
void child2_thread()
{
while (1)
{
led_on(LED_1);
printf_("child 2 thread 2222222\n");
led_off(LED_1);
timer_delay_ms(700);
}
}
static void tui_show_current_refresh_interval(void) { // This should be genericised when an another place needs to show a constant header + ram string buffer.
unsigned char buf[16];
lcd_clear();
lcd_gotoxy(2,0);
lcd_puts_P(PSTR("INTERVAL NOW:"));
tui_print_refresh_interval(buf,tui_update_refresh_interval());
lcd_gotoxy((16 - strlen((char*)buf))/2,1);
lcd_puts(buf);
timer_delay_ms(100);
tui_waitforkey();
}
static void tui_alarm_setstate(uint8_t t) {
if (t) {
backlight_simple_set(16);
#ifdef ALARMCLOCK
if (t==2) {
relay_set(RLY_MODE_ON);
} else { // 0.15s Bzzt (10% duty), 10ms period
for (uint8_t z = 0;z<15;z++) {
relay_set(RLY_MODE_ON);
timer_delay_ms(1);
relay_set(RLY_MODE_OFF);
timer_delay_ms(9);
}
}
#endif
} else {
#ifdef ALARMCLOCK
relay_set(RLY_MODE_OFF);
#endif
backlight_simple_set(0);
}
}
void child1_thread()
{
while (1)
{
char c = uart_read();
printf_("char >%c<\n", c);
led_on(LED_1);
printf_("child 1 thread 1111111\n");
led_off(LED_1);
timer_delay_ms(300);
}
}
short button_pressed( int button_number){
switch( button_number){
case 1:
if(BUTTON_PORT->DATA & (1 << BUTTON1_PIN)){
timer_delay_ms(DEB_TIME_MS);
if(BUTTON_PORT->DATA & (1 << BUTTON1_PIN))
return 1;
}else
return 0;
break;
case 2:
if(BUTTON_PORT->DATA & (1 << BUTTON2_PIN)){
timer_delay_ms(DEB_TIME_MS);
if(BUTTON_PORT->DATA & (1 << BUTTON2_PIN))
return 1;
}else
return 0;
break;
case 3:
if(BUTTON_PORT->DATA & (1 << BUTTON3_PIN)){
timer_delay_ms(DEB_TIME_MS);
if(BUTTON_PORT->DATA & (1 << BUTTON3_PIN))
return 1;
}else
return 0;
break;
case 4:
if(BUTTON_PORT->DATA & (1 << BUTTON4_PIN)){
timer_delay_ms(DEB_TIME_MS);
if(BUTTON_PORT->DATA & (1 << BUTTON4_PIN))
return 1;
}else
return 0;
break;
}
return 0;
}
void main_thread()
{
led_on(LED_1);
printf_(OS_WELCOME_MESSAGE);
create_thread(child1_thread, child1_thread_stack, sizeof(child1_thread_stack), PRIORITY_MAX);
create_thread(child2_thread, child2_thread_stack, sizeof(child2_thread_stack), PRIORITY_MAX);
while (1)
{
led_on(LED_1);
printf_("main thread\n");
led_off(LED_1);
timer_delay_ms(500);
}
}
void main_thread()
{
printf_(OS_WELCOME_MESSAGE);
u32 res = hmc5883_init();
while (1)
{
led_on(LED_1);
hmc5883_read();
//printf_("%u : %i %i %i\n", res, g_hmc5883.mx, g_hmc5883.my, g_hmc5883.mz);
printf_("%u : %i \n", res, g_hmc5883.mz);
led_off(LED_1);
timer_delay_ms(100);
}
robot_main();
}
void move_to_cell(StepperA* motor, VerticalActuatorA* va, unsigned char cell) {
vertical_actuator_transition_tier(va, cell / CELLS_PER_TIER);
timer_delay_ms(DELAY_BETWEEN_MOVEMENTS_MS);
stepper_set_position(motor, stepper_degrees_to_position((cell % CELLS_PER_TIER) * DEGREES_BETWEEN_CELLS));
timer_delay_ms(DELAY_BETWEEN_MOVEMENTS_MS);
}
int main(void)
{
ADCON1 = 0x0F;
CMCON = 0x07;
TRISA = 0x00;
PORTA = 0x00;
interrupt_initialize();
timer_initialize();
nrf_initialize(nrf_transmit_mode, nrf_channel_0, CAFE_ADDRESS | NODE_ID);
nrf_packet_t packet;
uint8_t differ = 0x00;
PORTA = 0x00;
while(1)
{
#if CONSTANT_TRANSMIT
#if SEND_TO_APP
packet.address = CAFE_ADDRESS | HUB_ID;
packet.payload_length = 18;
packet.payload[0] = 0x05; // Branch
packet.payload[1] = (NODE_ID >> 8); // Source ID
packet.payload[2] = (NODE_ID & 0xFF);
packet.payload[3] = (HUB_ID >> 8); // Destination ID
packet.payload[4] = (HUB_ID & 0xFF);
packet.payload[5] = 0x00; // Source App
packet.payload[6] = 0x00;
packet.payload[7] = 0x80; // Destination App
packet.payload[8] = 0x41;
packet.payload[9] = 0x07; // Command
packet.payload[10] = 0x00; // Sub-Command
packet.payload[11] = 0x06; // Payload Length
packet.payload[12] = 0xba; // Payload
packet.payload[13] = 0xba;
packet.payload[14] = 0xde;
packet.payload[15] = 0xca;
packet.payload[16] = 0xfe;
packet.payload[17] = differ++;
nrf_set_mode(nrf_transmit_mode, true);
nrf_flush_fifo(true);
nrf_transmit_packet(packet, false);
while(!(nrf_read_register(NRF_REG_FIFO_STATUS) & 0x10));
PORTA ^= 0x01;
timer_delay_ms(1000);
#endif
#if SEND_TO_HUB_APP
packet.address = CAFE_ADDRESS | HUB_ID;
packet.payload_length = 12;
packet.payload[0] = 0x05; // Branch
packet.payload[1] = (NODE_ID >> 8); // Source ID
packet.payload[2] = (NODE_ID & 0xFF);
packet.payload[3] = (HUB_ID >> 8); // Destination ID
packet.payload[4] = (HUB_ID & 0xFF);
packet.payload[5] = 0x00; // Source App
packet.payload[6] = 0x00;
packet.payload[7] = 0x00; // Destination App
packet.payload[8] = 0x00;
packet.payload[9] = 0x04; // Command
packet.payload[10] = 0x05; // Sub-Command
packet.payload[11] = 0x00; // Payload Length
nrf_set_mode(nrf_transmit_mode, true);
nrf_flush_fifo(true);
nrf_transmit_packet(packet, false);
while(!(nrf_read_register(NRF_REG_FIFO_STATUS) & 0x10));
PORTA ^= 0x02;
timer_delay_ms(1000);
#endif
#if SEND_TO_HUB
packet.address = CAFE_ADDRESS;
packet.payload_length = 13;
packet.payload[0] = 0x05; // Branch
packet.payload[1] = (NODE_ID >> 8); // Source ID
packet.payload[2] = (NODE_ID & 0xFF);
packet.payload[3] = 0x00; // Destination ID
packet.payload[4] = 0x00;
packet.payload[5] = 0x00; // Source App
packet.payload[6] = 0x00;
packet.payload[7] = 0x00; // Destination App
packet.payload[8] = 0x00;
packet.payload[9] = 0x05; // Command
packet.payload[10] = 0x05; // Sub-Command
packet.payload[11] = 0x01; // Payload Length
packet.payload[11] = differ; // Payload
nrf_set_mode(nrf_transmit_mode, true);
nrf_flush_fifo(true);
nrf_transmit_packet(packet, false);
while(!(nrf_read_register(NRF_REG_FIFO_STATUS) & 0x10));
PORTA ^= 0x02;
timer_delay_ms(1000);
#endif
#if SEND_HUB_ID_REQUEST
packet.address = CAFE_ADDRESS | HUB_ID;
packet.payload_length = 13;
packet.payload[0] = 0x05; // Branch
packet.payload[1] = (NODE_ID >> 8); // Source ID
packet.payload[2] = (NODE_ID & 0xFF);
packet.payload[3] = (HUB_ID >> 8); // Destination ID
packet.payload[4] = (HUB_ID & 0xFF);
packet.payload[5] = 0x00; // Source App
packet.payload[6] = 0x00;
packet.payload[7] = 0x00; // Destination App
packet.payload[8] = 0x00;
packet.payload[9] = 0x06; // Command
packet.payload[10] = 0x05; // Sub-Command
packet.payload[11] = 0x01; // Payload Length
packet.payload[12] = 0x01; // Payload
nrf_set_mode(nrf_transmit_mode, true);
nrf_flush_fifo(true);
nrf_transmit_packet(packet, false);
while(!(nrf_read_register(NRF_REG_FIFO_STATUS) & 0x10));
PORTA ^= 0x02;
timer_delay_ms(1000);
#endif
#elif SEND_PACKETS
packet.address = CAFE_ADDRESS | HUB_ID;
packet.payload_length = 18;
packet.payload[0] = 0x05; // Branch
packet.payload[1] = (NODE_ID >> 8); // Source ID
packet.payload[2] = (NODE_ID & 0xFF);
packet.payload[3] = (HUB_ID >> 8); // Destination ID
packet.payload[4] = (HUB_ID & 0xFF);
packet.payload[5] = 0x00; // Source App
packet.payload[6] = 0x00;
packet.payload[7] = 0x00; // Destination App
packet.payload[8] = 0x02;
packet.payload[9] = 0x01; // Command
packet.payload[10] = 0x00; // Sub-Command
packet.payload[11] = 0x00; // Payload Length
PORTA ^= 0x01;
nrf_set_mode(nrf_transmit_mode, true);
nrf_flush_fifo(true);
nrf_transmit_packet(packet, false);
while(!(nrf_read_register(NRF_REG_FIFO_STATUS) & 0x10));
PORTA ^= 0x01;
nrf_set_mode(nrf_receive_mode, true);
nrf_flush_fifo(false);
uint8_t delay_count = 0;
PORTA ^= 0x02;
while((nrf_read_register(NRF_REG_FIFO_STATUS) & 0x01) && (delay_count < 40))
{
timer_delay_ms(25);
delay_count++;
}
PORTA ^= 0x02;
if(delay_count < 40)
{
if(nrf_read_packet(&packet) != SUCCESS)
delay_count = 40;
else
PORTA ^= 0x04;
}
if(delay_count < 40)
timer_delay_ms(2000);
timer_delay_ms(2000);
#elif RECEIVE_PACKETS
nrf_set_mode(nrf_receive_mode, false);
while(!(nrf_read_register(NRF_REG_FIFO_STATUS) & 0x01))
{
timer_delay_ms(50);
PORTA ^= 0x02;
nrf_read_packet(&packet);
uint16_t temp;
temp = (packet.payload[1] << 8);
temp |= packet.payload[2];
packet.address = CAFE_ADDRESS | temp;
packet.payload_length = 12;
packet.payload[1] = (NODE_ID >> 8);
packet.payload[2] = (NODE_ID & 0xFF);
packet.payload[3] = (temp >> 8);
packet.payload[4] = (temp & 0xFF);
temp = (packet.payload[5] << 8);
temp |= packet.payload[6];
packet.payload[5] = packet.payload[7];
packet.payload[6] = packet.payload[8];
packet.payload[7] = (temp >> 8);
packet.payload[8] = (temp & 0xFF);
packet.payload[9] ^= 0x80;
packet.payload[10] = differ++;
packet.payload[11] = 0x00;
PORTA ^= 0x01;
nrf_set_mode(nrf_transmit_mode, true);
nrf_flush_fifo(true);
nrf_transmit_packet(packet, false);
while(!(nrf_read_register(NRF_REG_FIFO_STATUS) & 0x10));
PORTA ^= 0x01;
nrf_set_mode(nrf_receive_mode, false);
nrf_write_register(NRF_REG_STATUS, 0x20);
}
#endif
}
}
void nrf24_port_delay_ms(uint16_t delay_ms)
{
timer_delay_ms(delay_ms);
}
static void tui_gen_message_end(void) {
timer_delay_ms(100);
tui_waitforkey();
}
static void tui_gen_message_end(void)
{
lcd_clear_eos();
timer_delay_ms(100);
tui_waitforkey();
}