void led_ctrl(void)
{
if((fly_param.led.cycle == 0) ||(fly_param.led.sequence == NULL) || (fly_param.led.sequence_time == NULL)) return;
if(fly_param.led.state == false)
{
fly_param.led.state = true;
led_update(*fly_param.led.sequence);
fly_param.led.event_time = fly_param.current_time;
return;
}
if((fly_param.current_time - fly_param.led.event_time) >= *fly_param.led.sequence_time)
{
fly_param.led.cycle--;
if(fly_param.led.cycle == 0)
{
led_init();
}
else
{
fly_param.led.sequence++;
fly_param.led.sequence_time++;
fly_param.led.event_time = fly_param.current_time;
}
led_update(*fly_param.led.sequence);
}
}
void roomba_t::setup(int BRC_pin)
{
// Bring baud rate control (BRC) pin low
// This wakes up the Open Interface so it listens for serial
pinMode(BRC_pin,OUTPUT);
digitalWrite(BRC_pin,HIGH); // BRC seems to be edge-triggered
roomba_delay(ROOMBA_SYNC_TIME);
digitalWrite(BRC_pin,LOW);
for (int attempt=0;attempt<10;attempt++)
{
sensor_packet_m.mode=0;
roomba_delay(ROOMBA_SYNC_TIME);
start();
set_mode(roomba_t::FULL);
set_led_clean(255,0xff); // orange == 128
led_update();
set_receive_sensors(true);
for (int wait=0;wait<20;wait++) {
update();
if (sensor_packet_m.mode!=0) { // success!
digitalWrite(BRC_pin,HIGH);
return;
}
roomba_delay(100);
}
// Well, that didn't work. Try reset command.
reset();
}
digitalWrite(BRC_pin,HIGH);
}
void led_set(int i, int on)
{
on &= 1;
led_setup &= ~(1 << i);
led_setup |= (on << i);
led_update();
}
void ui_state_title(void)
{
int x = 0, y = 0, dx = 1, dy = 1, w = 20, h = 20, i = 0;
snd_play("sounds/intro-valkyrie.raw");
while (1) {
image_draw(0, 0, img_bg);
image_draw(x, y, img_soldier);
screen_show_buffer();
if (y + h == SCREEN_HEIGHT && dy == 1) dy = -1;
if (y == 0 && dy == -1) dy = 1;
if (x + w == SCREEN_WIDTH && dx == 1) dx = -1;
if (x == 0 && dx == -1) dx = 1;
x += dx; y += dy;
led_setup = (i++ / 10) % 2 ? 0xaa : 0x55;
led_update();
if ((btn_ignore = btn_read()) > 0) {
ui_state = UI_GAME_SOLDIER;
game_init();
return;
}
}
}
int main(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
// disable JTAG，use SWD only
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
timebase_init();
led_init();
vcp_init();
// for debug use
if (!key_is_pressed()) {
USB_HW_Config();
USB_Init();
}
led_flash(1000, 100, 0);
while (1) {
led_update();
if (bDeviceState == CONFIGURED)
{
if (key_is_pressed()) {
GPIO_ResetBits(GPIOB, GPIO_Pin_2);
GPIO_ResetBits(GPIOB, GPIO_Pin_1);
while (key_is_pressed());
GPIO_SetBits(GPIOB, GPIO_Pin_2);
}
}
}
}
void led_init(void)
{
fly_param.led.state = false;
fly_param.led.cycle = 0;
fly_param.led.sequence = NULL;
fly_param.led.sequence_time = NULL;
led_update(LED_OFF);
}
void led_run_sequence(led_data_t *leds, led_seq_step_t *sequence, int32_t num_repeat)
{
leds->last_mode = leds->mode;
leds->mode = led_mode_sequence;
leds->sequence = sequence;
leds->seq_num_repeat = num_repeat;
led_set_sequence_step(leds, 0);
led_update(leds);
}
void sched_update (void)
{
t1ms_cnt++;
if (t1ms_cnt == 10000)
t1ms_cnt = 0;
if (t1ms_cnt % 20 == 0) /* each 20 ms */
{
wheel_update_ticks_buffers();
}
if (t1ms_cnt % pid_interval == 0) /* motor controller update */
{
if (pid_enable)
wheel_update_pid(); /* update PID motor controller */
else
wheel_update_open_loop(); /* update open loop motor controller */
}
if (t1ms_cnt % pfbst_interval == 0) /* send $PFBST */
{
nmea_tx_status();
}
/* each 10 ms */
if (t1ms_cnt % 10 == 0) /* each 10 ms */
{
if (t1ms_cnt % 20 == 0) /* each 20 ms */
{
}
if (t1ms_cnt % 50 == 0) /* each 50 ms */
{
}
if (t1ms_cnt % 100 == 0) /* each 100 ms */
{
if (nmea_wd_timeout)
nmea_wd++; /* increase watchdog timeout */
else
nmea_wd = 0;
voltage = adc_data[0]; /* request voltage measurement */
state_update();
}
if (t1ms_cnt % 200 == 0) /* each 200 ms */
{
button_update();
led_update();
voltage_update();
}
}
}
void led_indicate_trx(led_data_t *leds, led_num_t num)
{
uint32_t now = HAL_GetTick();
led_state_t *led = &leds->led_state[num];
if ( (led->on_until < now) && (led->off_until < now) ) {
led->off_until = now + 30;
led->on_until = now + 45;
}
led_update(leds);
}
void vUiTask( void * pvParameters)
{
lcd_s_reset();
// int_lcd_timer_dec = 1;
#ifdef DEBUG_BUILD
#else
led_update();
printf_P(PSTR("DG1YFE"));
lcd_fill();
lcd_cpos(0);
printf_P(PSTR("MCmega"));
lcd_fill();
lcd_cpos(0);
vTaskDelay(150);
printf_P(version_str);
lcd_fill();
vTaskDelay(150);
// reset_ui(UI_RESET_WARM);
#endif
reset_ui(UI_RESET_COLD);
for(;;)
{
pll_led(0);
led_update();
menu();
taskYIELD();
// check if reset of control head was detected
// (certain amount of 0x7e reset messages was received)
if(!ch_reset_detected)
{
lcd_s_reset();
reset_ui(UI_RESET_COLD);
}
config_validate();
}
}
uint8_t matrix_scan() {
// Update LED states if necessary:
#ifdef LED_CONTROLLER_ENABLE
led_update();
#endif
for ( uint8_t i = 0; i < MATRIX_ROWS; i++ ) {
select_row( i );
_delay_us( 30 ); // without this wait read unstable value.
matrix_row_t cols = read_cols();
if ( matrix_debouncing[ i ] != cols ) {
matrix_debouncing[ i ] = cols;
if ( debouncing ) {
debug("bounce!: ");
debug_hex( debouncing );
debug( "\n" );
}
debouncing = DEBOUNCE;
}
unselect_rows();
}
if ( debouncing ) {
if ( --debouncing ) {
_delay_ms( 1 );
} else {
for ( uint8_t i = 0; i < MATRIX_ROWS; i++ ) {
matrix[ i ] = matrix_debouncing[ i ];
}
}
}
return 1;
}
int main (void)
{
led_init();
i2cinit();
servo_init(); //this will initialize servo control, but power supply of all servos will be disabled for safety reasons. first set the correct angulars of all servos over twi and issue a servo power up afterwards (CMD_SERVOSonoff)
//NOTE: Servo power control can be overridden by the jumper near the mosfet (jumper plugged in = override)
led1_an;
_delay_ms(500);
led1_aus;
_delay_ms(500);
led1_an;
_delay_ms(500);
led1_aus;
led2_aus;
sei(); //enable global interrupts
longcount = 0;
TCCR2 = (1<<CS22) | (1<<CS21) | (1<<CS20); // servo watchdog prescaler 1024
while (1) {
if(TCNT2>100){ // >=0.0128s
TCNT2 = 0;
longcount++;
if(longcount>40){ //>=0.512 s
longcount = 0;
uint8_t i;
//reset servos
for(i = 0; i<8; i++){
servos_angular[i] = 4000;
}
}
}
//handle status leds
led_update();
}
}
void led_set_all(int new_setup)
{
int i, j;
led_setup = 0;
for (i = 0; i < 8; i++) {
if ((new_setup & (1 << i)) == 0)
continue;
j = i;
switch (i) {
case 3:
case 4:
case 5:
case 6: j += 2; break;
case 7: j = 22; break;
}
led_setup |= (1 << j);
}
led_update();
}
static void
led_0(void *arg, int onoff)
{
led_update(arg, 0, onoff);
}
static void
led_7(void *arg, int onoff)
{
led_update(arg, 7, onoff);
}
static void
led_6(void *arg, int onoff)
{
led_update(arg, 6, onoff);
}
static void
led_5(void *arg, int onoff)
{
led_update(arg, 5, onoff);
}
int main(void)
{
init();
sei();
if(wdt_wasReset())
resetError();
buzzer_buzz(200, TONE_4KHZ, VOL_UI);
led_flash(LED_GREEN, 50, 255);
watchface_setFace(watchface_normal);
watchface_loadFace();
/*
while(1)
{
buzzer_buzzb(200,TONE_5KHZ, VOL_UI);
buzzer_buzzb(200,TONE_4KHZ, VOL_UI);
buzzer_buzzb(200,TONE_2_5KHZ, VOL_UI);
buzzer_buzzb(200,TONE_2KHZ, VOL_UI);
buzzer_buzzb(200,TONE_4KHZ, VOL_UI);
buzzer_buzzb(200,TONE_5KHZ, VOL_UI);
buzzer_buzzb(200,TONE_2_5KHZ, VOL_UI);
buzzer_buzzb(200,TONE_2KHZ, VOL_UI);
buzzer_buzzb(200,TONE_3_5KHZ, VOL_UI);
buzzer_buzzb(200,TONE_4KHZ, VOL_UI);
buzzer_buzzb(200,TONE_3KHZ, VOL_UI);
buzzer_buzzb(250,TONE_2KHZ, VOL_UI);
buzzer_buzzb(250,TONE_2KHZ, VOL_UI);
buzzer_buzzb(200,TONE_4KHZ, VOL_UI);
buzzer_buzzb(200,TONE_4KHZ, VOL_UI);
}
*/
while(1)
{
bool timeUpt = time_update();
if(pwrmgr_userActive())
{
if(timeUpt && timeData.secs == 0)
battery_update();
buttons_update();
}
buzzer_update();
led_update();
stopwatch_update();
if(pwrmgr_userActive())
{
alarm_update();
display_update();
}
// freeRAM();
wdt_update();
pwrmgr_update();
}
}
void led_set_mode(led_data_t *leds,led_mode_t mode)
{
leds->mode = mode;
led_update(leds);
}
void timHandler(void)
{
led_update();
led_updated = 1;
}
void interrupt tmr0_isr (void) {
led_update();
T0IF = 0;
TMR0 = PW_period;
}
static void
led_1(void *arg, int onoff)
{
led_update(arg, 1, onoff);
}
static void
led_3(void *arg, int onoff)
{
led_update(arg, 3, onoff);
}
int
main(int argc, char** argv)
{
uint32_t led_colors = 0;
uint8_t at_parm_test[10];
unsigned once;
uint32_t i = 0;
uint8_t canPrescaler = 0;
uint8_t* uart_tx_packet = 0;
uint8_t* uart_rx_packet;
uint32_t old_loadcell_data;
uint16_t timeStep = 1;
clock_init();
pin_init();
led_init();
timers_init();
state_init();
uart_init();
// Set up UART2 for 115200 baud. There's no round() on the dsPICs, so we implement our own.
double brg = (double) 140000000 / 2.0 / 16.0 / 115200.0 - 1.0;
if (brg - floor(brg) >= 0.5) {
brg = ceil(brg);
}
else {
brg = floor(brg);
}
// Uart2Init (brg); // Init UART 2 as 115200 baud/s
loadcell_init();
loadcell_start();
led_rgb_off();
led_rgb_set(50, 0, 100);
can_state.init_return = RET_UNKNOWN;
if (can_init()) {
while (1);
}
timer_state.prev_systime = 0;
timer_state.systime = 0;
#ifdef CONF71
// Start Reading the int pin on IMU
mpuData.startData = 0;
if (IMU_Init(400000, 70000000) == 0) {
// imu_state.init_return = RET_OK;
mpuData.startData = 1;
}
else {
//imu_state.init_return = RET_ERROR;
}
#endif
for (;;) {
if (timer_state.systime != timer_state.prev_systime) {
timer_state.prev_systime = timer_state.systime;
//everything in here will be executed once every ms
//make sure that everything in here takes less than 1ms
//useful for checking state consistency, synchronization, watchdog...
if(timer_state.systime % 1000 == 1) {
LED_4 = 1;
}
led_update();
if (timer_state.systime % 10 == 1) {
IMU_GetQuaternion(quaterion);
QuaternionToYawPitchRoll(quaterion, ypr);
}
if (timer_state.systime % 5 == 1) {
IMU_normalizeData(&mpuData, &magData, &imuData);
// Run AHRS algorithm
IMU_UpdateAHRS (&imuData);
// Run IMU algorithm (does not use MAG data)
// IMU_UpdateIMU(&imuData);
//copy state to CAN dictionary
IMU_CopyOutput(&imuData, &mpuData, &magData);
}
/**
* CANFestival Loop
*/
if (can_state.init_return == RET_OK) {
can_process();
/**
* Sets CANFestival shared variables
* specific to Sensor Board
*/
can_push_state();
}
/**
* Blinking LED Loop
*/
if (timer_state.systime % 25 == 0) {
LED_1 = !LED_1;
}
}
else {
//untimed processes in main loop:
//executed as fast as possible
//these processes should NOT block the main loop
// LED_4 = mpuData.accelX > 0;
// LED_3 = mpuData.accelY > 0;
// LED_1 = mpuData.accelZ > 0;
// IMU_GetData();
IMU_CopyI2CData(&mpuData, &magData);
if (!T1CONbits.TON) {
RGB_RED = 0;
RGB_GREEN = RGB_BLUE = 1;
while (1);
}
if(can_flag){
TimeDispatch();
can_flag = 0;
}
uart_rx_packet = uart_rx_cur_packet();
if (uart_rx_packet != 0) {
uart_rx_packet_consumed();
}
/**
* Handles CAN transmission buffers
*/
if ((txreq_bitarray & 0b00000001) && !C1TR01CONbits.TXREQ0) {
C1TR01CONbits.TXREQ0 = 1;
txreq_bitarray = txreq_bitarray & 0b11111110;
}
if ((txreq_bitarray & 0b00000010) && !C1TR01CONbits.TXREQ1) {
C1TR01CONbits.TXREQ1 = 1;
txreq_bitarray = txreq_bitarray & 0b11111101;
}
if ((txreq_bitarray & 0b00000100) && !C1TR23CONbits.TXREQ2) {
C1TR23CONbits.TXREQ2 = 1;
txreq_bitarray = txreq_bitarray & 0b11111011;
}
if ((txreq_bitarray & 0b00001000) && !C1TR23CONbits.TXREQ3) {
C1TR23CONbits.TXREQ3 = 1;
txreq_bitarray = txreq_bitarray & 0b11110111;
}
if ((txreq_bitarray & 0b00010000) && !C1TR45CONbits.TXREQ4) {
C1TR45CONbits.TXREQ4 = 1;
txreq_bitarray = txreq_bitarray & 0b11101111;
}
if ((txreq_bitarray & 0b00100000) && !C1TR45CONbits.TXREQ5) {
C1TR45CONbits.TXREQ5 = 1;
txreq_bitarray = txreq_bitarray & 0b11011111;
}
if ((txreq_bitarray & 0b01000000) && !C1TR67CONbits.TXREQ6) {
C1TR67CONbits.TXREQ6 = 1;
txreq_bitarray = txreq_bitarray & 0b10111111;
}
}
}
return (EXIT_SUCCESS);
}
/*
* Application entry point.
*/
int main(void)
{
enum led_status lstat = LST_INIT;
EventListener el0;
alert_status_t proto_st = ALST_INIT;
alert_status_t bmp085_st = ALST_INIT;
alert_status_t mpu6050_st = ALST_INIT;
alert_status_t hmc5883_st = ALST_INIT;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
#ifdef BOARD_IMU_AHRF
/* Clear DRDY pad */
palClearPad(GPIOA, GPIOA_DRDY);
/* Activates serial */
sdStart(&SD1, NULL);
sdStart(&SD2, NULL);
/* Activate pwm */
pwmStart(&PWMD1, &pwm1cfg);
/* Activate i2c */
i2cStart(&I2CD1, &i2c1cfg);
/* Activate exti */
extStart(&EXTD1, &extcfg);
#endif /* BOARD_IMU_AHRF */
#ifdef BOARD_CAPTAIN_PRO2
/* Activates serial */
sdStart(&SD3, NULL);
sdStart(&SD4, NULL);
/* Activate pwm */
pwmStart(&PWMD3, &pwm3cfg);
pwmStart(&PWMD4, &pwm4cfg);
pwmStart(&PWMD5, &pwm5cfg);
/* Activate i2c */
i2cStart(&I2CD1, &i2c1cfg);
/* Activate exti */
extStart(&EXTD1, &extcfg);
/* Activate adc */
adcStart(&ADCD1, NULL);
#endif /* BOARD_CAPTAIN_PRO2 */
/* alert subsys */
chEvtInit(&alert_event_source);
chEvtRegister(&alert_event_source, &el0, 0);
/* init devices */
pt_init();
chThdSleepMilliseconds(10); /* power on delay */
#ifdef HAS_DEV_BMP085
bmp085_init();
chThdSleepMilliseconds(50); /* init delay */
#endif
#ifdef HAS_DEV_MS5611
ms5611_init(&ms5611cfg);
chThdSleepMilliseconds(50); /* init delay */
#endif
#ifdef HAS_DEV_MPU6050
mpu6050_init(&mpu6050cfg);
chThdSleepMilliseconds(250); /* give some time for mpu6050 configuration */
#endif
#ifdef HAS_DEV_HMC5883
hmc5883_init(&hmc5883cfg);
#endif
#ifdef HAS_DEV_SERVOPWM
servopwm_init(&servopwmcfg);
#endif
#ifdef HAS_DEV_NTC10K
ntc10k_init();
#endif
#ifdef HAS_DEV_RPM
rpm_init();
#endif
#ifdef BOARD_IMU_AHRF
/* Set DRDY pad */
palSetPad(GPIOA, GPIOA_DRDY);
#endif
while (TRUE) {
eventmask_t msk = chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(100));
if (msk & EVENT_MASK(0)) {
flagsmask_t fl = chEvtGetAndClearFlags(&el0);
if (fl & ALERT_FLAG_PROTO)
proto_st = pt_get_status();
#ifdef HAS_DEV_MPU6050
if (fl & ALERT_FLAG_MPU6050)
mpu6050_st = mpu6050_get_status();
#endif
#ifdef HAS_DEV_HMC5883
if (fl & ALERT_FLAG_HMC5883)
hmc5883_st = hmc5883_get_status();
#endif
#ifdef HAS_DEV_BMP085
if (fl & ALERT_FLAG_BMP085)
bmp085_st = bmp085_get_status();
#endif
#ifdef HAS_DEV_MS5611
if (fl & ALERT_FLAG_BMP085)
bmp085_st = ms5611_get_status();
#endif
pt_set_sens_state(mpu6050_st, hmc5883_st, bmp085_st);
}
if (proto_st == ALST_FAIL || mpu6050_st == ALST_FAIL || hmc5883_st == ALST_FAIL || bmp085_st == ALST_FAIL)
lstat = LST_FAIL;
else if (proto_st == ALST_INIT || mpu6050_st == ALST_INIT || hmc5883_st == ALST_INIT || bmp085_st == ALST_INIT)
lstat = LST_INIT;
else if (proto_st == ALST_NORMAL && mpu6050_st == ALST_NORMAL && hmc5883_st == ALST_NORMAL && bmp085_st == ALST_NORMAL)
lstat = LST_NORMAL;
led_update(lstat);
}
}
static void
led_2(void *arg, int onoff)
{
led_update(arg, 2, onoff);
}
void led_clear(void)
{
led_setup = 0;
led_update();
}
static void
led_4(void *arg, int onoff)
{
led_update(arg, 4, onoff);
}
void interrupt tmr0_isr (void) {
led_update();
T0IF = 0;
TMR0 = 200;
}
