int main() { init_motors(); init_peripherals(); // find out how many cells are connected u16 batteryCutoff; while((batteryCutoff = getBatteryVoltage()) == 0); if(batteryCutoff > 720) // voltage is higher than 6V -> 2cells batteryCutoff = 720; // cutoff 6V else batteryCutoff = 360; // cutoff 3V calibrate_sensors(); // enable interrupt driven gyro acquisation startGyro(); u08 c = 0, maxThrust = (batteryCutoff>360)?70:100; u08 cutBattery = 0; while(1) { if(Thrust > maxThrust) Thrust = maxThrust; calculate_balance(Thrust, Yaw, Pitch, Roll); checkForInput(); // check battery if(((c%100)==0)) { u16 bat = getBatteryVoltage(); if(bat > 0 && bat <= batteryCutoff) { if(maxThrust > 0) maxThrust -= 5; cutBattery = 1; } } if(!cutBattery || !(c % 50)) PORTC ^= (1<<2); c++; //_delay_ms(100); } }
void app_main() { init_peripherals(); motor_a = new Motor(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM0A, MCPWM0B, GPIO_PWM0A_OUT, GPIO_PWM0B_OUT); motor_b = new Motor(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM1A, MCPWM1B, GPIO_PWM1A_OUT, GPIO_PWM1B_OUT); printf("Press a key to enter console\n"); bool debug = false; for (int i = 0; i < 20; ++i) { if (getchar() != EOF) { debug = true; break; } vTaskDelay(100/portTICK_PERIOD_MS); } if (debug) run_console(); // never returns printf("\nStarting application\n"); xTaskCreate(&main_loop, "Main loop", 10240, NULL, 1, &xMainTask); }
int main() { uint8 i2c_flag = 0; uint16 tmp_volt_3v3 = 0, tmp_volt_vg = 0, tmp_volt_vb = 0; uint16 extend_error_pulse = 0; //Initialize and start peripherals: init_peripherals(); // Enable global interrupts CyGlobalIntEnable; //PWM pass-through (ToDo program safety feature) PWM_Enable_Write(1); //Disable shorted leads SL_EN_Write(0); while(1) { //For now we always generate the -4V5: SL_CLK_Write(1); CyDelayUs(5); SL_CLK_Write(0); //Update sensor data every 10ms, call safety functions: if(flag_tb10ms) { flag_tb10ms = 0; //Update shared memory: ezI2Cbuf[MEM_R_VB_SNS] = read_vb(); ezI2Cbuf[MEM_R_VG_SNS] = read_vg(); ezI2Cbuf[MEM_R_5V_SNS] = 0; // TBD ezI2Cbuf[MEM_R_3V3_SNS] = read_3v3(); ezI2Cbuf[MEM_R_TEMPERATURE] = read_temp(); //ezI2Cbuf[MEM_R_STATUS1] = STATUS1_GOOD; //Safety functions: err_temp = safety_temp(ezI2Cbuf[MEM_R_TEMPERATURE]); err_v_3v3 = safety_volt(ezI2Cbuf[MEM_R_3V3_SNS], M_3V3_LOW, M_3V3_HIGH); err_v_vg = safety_volt(ezI2Cbuf[MEM_R_VG_SNS], M_VG_LOW, M_VG_HIGH); err_v_vb = safety_volt(ezI2Cbuf[MEM_R_VB_SNS], M_VB_LOW, M_VB_HIGH); err_discon = safety_disconnection(ezI2Cbuf[MEM_R_VB_SNS]); ezI2Cbuf[MEM_R_STATUS1] = CMB_FLAGS_STATUS1(err_wdclk, err_discon, err_temp, err_v_vb, err_v_vg); ezI2Cbuf[MEM_R_STATUS2] = CMB_FLAGS_STATUS2(err_v_3v3); } if(flag_tb_1ms) { flag_tb_1ms = 0; if(flag_wdclk == 1) //Watchdog clock expired { extend_error_pulse = 1000; led_period = LED_PERIOD_ERROR; flag_wdclk = 2; //2 means "being processed" err_wdclk = 1; } else { led_period = LED_PERIOD_NORM; } if(extend_error_pulse >= 1) { extend_error_pulse--; } else { extend_error_pulse = 0; flag_wdclk = 0; err_wdclk = 0; } } //EZI2C Write complete if (0u != (I2C_1_EzI2CGetActivity() & I2C_1_EZI2C_STATUS_WRITE1)) { //... i2c_flag = 1; i2c_flag = 0; } } }
//////////////////////////////////////////////////////////////////////////////// /// @brief Application main function. //////////////////////////////////////////////////////////////////////////////// void main(void) { // Initializations SET_MAIN_CLOCK_SOURCE(CRYSTAL); SET_MAIN_CLOCK_SPEED(MHZ_26); CLKCON = (CLKCON & 0xC7); init_peripherals(); P0 &= ~0x40; // Pulse the Codec Reset line (high to low, low to high) P0 |= 0x40; init_codec(); // Initilize the Codec INT_SETFLAG(INUM_DMA, INT_CLR); // clear the DMA interrupt flag I2SCFG0 |= 0x01; // Enable the I2S interface DMA_SET_ADDR_DESC0(&DmaDesc0); // Set up DMA configuration table for channel 0 DMA_SET_ADDR_DESC1234(&DmaDesc1_4[0]); // Set up DMA configuration table for channels 1 - 4 dmaMemtoMem(AF_BUF_SIZE); // Set up DMA Channel 0 for memmory to memory data transfers initRf(); // Set radio base frequency and reserve DMA channels 1 and 2 for RX/TX buffers dmaAudio(); // Set up DMA channels 3 and 4 for the Audio In/Out buffers DMAIRQ = 0; DMA_ARM_CHANNEL(4); // Arm DMA channel 4 macTimer3Init(); INT_ENABLE(INUM_T1, INT_ON); // Enable Timer 1 interrupts INT_ENABLE(INUM_DMA, INT_ON); // Enable DMA interrupts INT_GLOBAL_ENABLE(INT_ON); // Enable Global interrupts MAStxData.macPayloadLen = TX_PAYLOAD_LEN; MAStxData.macField = MAC_ADDR; while (1) { // main program loop setChannel(channel[band][ActiveChIdx]); // SetChannel will set the MARCSTATE to IDLE ActiveChIdx = (ActiveChIdx + 1) & 0x03; SCAL(); // Start PLL calibration at new channel if ((P1 & 0x08) != aux_option_status) { // if the 'SEL AUX IN' option bit has changed state if ((P1 & 0x08) == 0) { // SEL AUX IN has changed state to true I2Cwrite(MIC1LP_LEFTADC, 0xFC); // Disconnect MIC1LP/M from the Left ADC, Leave Left DAC enabled I2Cwrite(MIC2L_MIC2R_LEFTADC, 0x2F); // Connect AUX In (MIC2L) to Left ADC I2Cwrite(LEFT_ADC_PGA_GAIN, 0x00); // Set PGA gain to 0 dB aux_option_status &= ~0x08; } else { // SEL AUX IN has changed state to false I2Cwrite(MIC2L_MIC2R_LEFTADC, 0xFF); // Disconnect AUX In (MIC2L) from Left ADC I2Cwrite(MIC1LP_LEFTADC, 0x84); // Connect the internal microphone to the Left ADC using differential inputs (gain = 0 dB); Power Up the Left ADC I2Cwrite(LEFT_ADC_PGA_GAIN, 0x3C); // Enable PGA and set gain to 30 dB aux_option_status |= 0x08; } } if ((P1 & 0x04) != agc_option_status) { // if the 'ENA AGC' option bit has changed state if ((P1 & 0x04) == 0) { // ENA AGC has changed state to true I2Cwrite(LEFT_AGC_CNTRL_A, 0x90); // Left AGC Control Register A - Enable, set target level to -8 dB I2Cwrite(LEFT_AGC_CNTRL_B, 0xC8); // Left AGC Control Register B - Set maximum gain to to 50 dB I2Cwrite(LEFT_AGC_CNTRL_C, 0x00); // Left AGC Control Register C - Disable Silence Detection agc_option_status &= ~0x04; } else { // SEL AUX IN has changed state to false I2Cwrite(LEFT_AGC_CNTRL_A, 0x10); // Left AGC Control Register A - Disable agc_option_status |= 0x04; } } // Check the band selection bits band = 2; // if the switch is not in position 1 or 2, in must be in position 3 if ((P1 & 0x10) == 0) // check if switch is in position 1 band = 0; else if ((P0 & 0x04) == 0) // check if switch is in position 2 band = 1; // Now wait for the "audio frame ready" signal while (audioFrameReady == FALSE); // Wait until an audioframe is ready to be transmitted audioFrameReady = FALSE; // Reset the flag // Move data from the CODEC (audioOut) buffer to the TX buffer using DMA Channel 0 SET_WORD(DmaDesc0.SRCADDRH, DmaDesc0.SRCADDRL, audioOut[activeOut]); SET_WORD(DmaDesc0.DESTADDRH, DmaDesc0.DESTADDRL, MAStxData.payload); DmaDesc0.SRCINC = SRCINC_1; // Increment Source address DMAARM |= DMA_CHANNEL_0; DMAREQ |= DMA_CHANNEL_0; // Enable memory-to-memory transfer using DMA channel 0 while ((DMAARM & DMA_CHANNEL_0) > 0); // Wait for transfer to complete while (MARCSTATE != 0x01); // Wait for calibration to complete P2 |= 0x08; // Debug - Set P2_3 (TP2) rfSendPacket(MASTER_TX_TIMEOUT_WO_CALIB); P2 &= ~0x08; // Debug - Reset P2_3 (TP2) } // end of 'while (1)' loop }
int main() { uint8 i2c_flag = 0; uint16 tmp_volt_3v3 = 0, tmp_volt_vg = 0, tmp_volt_vb = 0; uint16 extend_error_pulse = 0; uint8 togg_eled = 0; //Initialize and start peripherals: init_peripherals(); //Enable global interrupts CyGlobalIntEnable; //Quick test: LED Green when ON LED_B_Write(1); //Test: latch output: SW_PSOC_Write(1); while(1) { //Update sensor data every 10ms, call safety functions: if(flag_tb10ms) { flag_tb10ms = 0; /* //Update shared memory: tmp_volt_vb = read_vb(); ezI2Cbuf[MEM_R_VB_SNS_MSB] = (uint8)((tmp_volt_vb & 0xFF00) >> 8); ezI2Cbuf[MEM_R_VB_SNS_LSB] = (uint8)(tmp_volt_vb & 0x00FF); tmp_volt_vg = read_vg(); ezI2Cbuf[MEM_R_VG_SNS_MSB] = (uint8)((tmp_volt_vg & 0xFF00) >> 8); ezI2Cbuf[MEM_R_VG_SNS_LSB] = (uint8)(tmp_volt_vg & 0x00FF); tmp_volt_3v3 = read_3v3(); ezI2Cbuf[MEM_R_3V3_SNS_MSB] = (uint8)((tmp_volt_3v3 & 0xFF00) >> 8); ezI2Cbuf[MEM_R_3V3_SNS_LSB] = (uint8)(tmp_volt_3v3 & 0x00FF); ezI2Cbuf[MEM_R_TEMPERATURE] = read_temp(); //ezI2Cbuf[MEM_R_STATUS1] = STATUS1_GOOD; //Safety functions: err_temp = safety_temp(ezI2Cbuf[MEM_R_TEMPERATURE]); err_v_3v3 = safety_volt(tmp_volt_3v3, M_3V3_LOW, M_3V3_HIGH); err_v_vg = safety_volt(tmp_volt_vg, M_VG_LOW, M_VG_HIGH); err_v_vb = safety_volt(tmp_volt_vb, M_VB_LOW, M_VB_HIGH); err_discon = safety_disconnection(tmp_volt_vb); ezI2Cbuf[MEM_R_STATUS1] = CMB_FLAGS_STATUS1(err_wdclk, err_discon, err_temp, err_v_vb, err_v_vg); ezI2Cbuf[MEM_R_STATUS2] = CMB_FLAGS_STATUS2(err_v_3v3); */ } if(flag_tb_1ms) { flag_tb_1ms = 0; } //EZI2C Write complete if (0u != (I2C_1_EzI2CGetActivity() & I2C_1_EZI2C_STATUS_WRITE1)) { //... i2c_flag = 1; i2c_flag = 0; } } }
int main(void) { //Local variables: uint8 i = 0; unsigned char result = 0; uint8 toggle_wdclk = 0; uint8 cmd_ready_485_1 = 0, cmd_ready_usb = 0; static uint8 new_cmd_led = 0; uint16 safety_delay = 0; uint8 i2c_time_share = 0; //Power on delay with LEDs power_on(); //Initialize all the peripherals init_peripherals(); //Start with an empty buffer flexsea_clear_slave_read_buffer(); //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- //Blocking Test code - enable one and only one for special //debugging. Normal code WILL NOT EXECUTE when this is enabled! //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //strain_test_blocking(); //safety_cop_comm_test_blocking(); //imu_test_code_blocking(); //motor_fixed_pwm_test_code_blocking(141); //wdclk_test_blocking(); //timing_test_blocking(); //test_current_tracking_blocking(); //test_pwm_pulse_blocking(); //test_uart_dma_xmit(); //motor_cancel_damping_test_code_blocking(); //csea_knee_up_down_test_demo(); //motor_stepper_test_blocking_1(80); //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //Non-Blocking Test code //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= #ifdef USE_SPI_COMMUT motor_stepper_test_init(0); //Note: deadtime is 55, small PWM values won't make it move. //Starting at 0, GUI will change that when it wants. #endif //USE_SPI_COMMUT //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //Special code for the ExoBoots: #ifdef PROJECT_EXOCUTE init_exo(); #endif //Main loop while(1) { if(t1_new_value == 1) { //If the time share slot changed we run the timing FSM. Refer to //timing.xlsx for more details. 't1_new_value' updates at 10kHz, //each slot at 1kHz. t1_new_value = 0; //Timing FSM: switch(t1_time_share) { //Case 0: I2C case 0: i2c_time_share++; i2c_time_share %= 4; #ifdef USE_I2C_INT //Subdivided in 4 slots. switch(i2c_time_share) { //Case 0.0: Accelerometer case 0: #ifdef USE_IMU get_accel_xyz(); i2c_last_request = I2C_RQ_ACCEL; #endif //USE_IMU break; //Case 0.1: Gyroscope case 1: #ifdef USE_IMU get_gyro_xyz(); i2c_last_request = I2C_RQ_GYRO; #endif //USE_IMU break; //Case 0.2: Safety-Cop case 2: safety_cop_get_status(); i2c_last_request = I2C_RQ_SAFETY; break; //Case 0.3: Free case 3: //I2C RGB LED //minm_test_code(); update_minm_rgb(); //ToDo: That's EXT_I2C, not INT break; default: break; } #endif //USE_I2C_INT #ifdef USE_SPI_COMMUT angle = as5047_read_single(AS5047_REG_ANGLEUNC); #endif //USE_SPI_COMMUT break; //Case 1: case 1: break; //Case 2: case 2: break; //Case 3: Strain Gauge DelSig ADC, SAR ADC case 3: //Start a new conversion ADC_DelSig_1_StartConvert(); //Filter the previous results strain_filter_dma(); break; //Case 4: User Interface case 4: //Alive LED alive_led(); //UI RGB LED: if(safety_delay > SAFETY_DELAY) { //status_error_codes(safety_cop.status1, safety_cop.status2, &eL0, &eL1, &eL2); } else { safety_delay++; } //Display temperature status on RGB overtemp_error(&eL1, &eL2); //Comment this line if safety code is problematic rgb_led_ui(eL0, eL1, eL2, new_cmd_led); //ToDo add more error codes if(new_cmd_led) { new_cmd_led = 0; } break; //Case 5: Quadrature encoder & Position setpoint case 5: #ifdef USE_QEI1 //Refresh encoder readings encoder_read(); #endif //USE_QEI1 #ifdef USE_TRAPEZ if((ctrl.active_ctrl == CTRL_POSITION) || (ctrl.active_ctrl == CTRL_IMPEDANCE)) { //Trapezoidal trajectories (can be used for both Position & Impedance) ctrl.position.setp = trapez_get_pos(steps); //New setpoint ctrl.impedance.setpoint_val = trapez_get_pos(steps); //New setpoint } #endif //USE_TRAPEZ break; //Case 6: P & Z controllers, 0 PWM case 6: #ifdef USE_TRAPEZ if(ctrl.active_ctrl == CTRL_POSITION) { motor_position_pid(ctrl.position.setp, ctrl.position.pos); } else if(ctrl.active_ctrl == CTRL_IMPEDANCE) { //Impedance controller motor_impedance_encoder(ctrl.impedance.setpoint_val, ctrl.impedance.actual_val); } #endif //USE_TRAPEZ //If no controller is used the PWM should be 0: if(ctrl.active_ctrl == CTRL_NONE) { motor_open_speed_1(0); } break; case 7: #ifdef USE_SPI_COMMUT //Stepper test code: motor_stepper_test_runtime(10); #endif //USE_SPI_COMMUT break; //Case 8: SAR ADC filtering case 8: filter_sar_adc(); break; //Case 9: User functions case 9: //ExoBoot code - 1kHz #ifdef PROJECT_EXOCUTE exo_fsm(); #endif break; default: break; } //The code below is executed every 100us, after the previous slot. //Keep it short! //BEGIN - 10kHz Refresh //RS-485 Byte Input #ifdef USE_RS485 //get_uart_data(); //Now done via DMA if(data_ready_485_1) { data_ready_485_1 = 0; //Got new data in, try to decode cmd_ready_485_1 = unpack_payload_485_1(); } #endif //USE_RS485 //USB Byte Input #ifdef USE_USB get_usb_data(); if(data_ready_usb) { data_ready_usb = 0; //Got new data in, try to decode cmd_ready_usb = unpack_payload_usb(); eL1 = 1; } #endif //USE_USB //FlexSEA Network Communication #ifdef USE_COMM //Valid communication from RS-485 #1? if(cmd_ready_485_1 != 0) { cmd_ready_485_1 = 0; //Cheap trick to get first line //ToDo: support more than 1 for(i = 0; i < PAYLOAD_BUF_LEN; i++) { tmp_rx_command_485_1[i] = rx_command_485_1[0][i]; } //payload_parse_str() calls the functions (if valid) result = payload_parse_str(tmp_rx_command_485_1); //LED: if(result == PARSE_SUCCESSFUL) { //Green LED only if the ID matched and the command was known new_cmd_led = 1; } } //Valid communication from USB? if(cmd_ready_usb != 0) { cmd_ready_usb = 0; //Cheap trick to get first line //ToDo: support more than 1 for(i = 0; i < PAYLOAD_BUF_LEN; i++) { tmp_rx_command_usb[i] = rx_command_usb[0][i]; } //payload_parse_str() calls the functions (if valid) result = payload_parse_str(tmp_rx_command_usb); //LED: if(result == PARSE_SUCCESSFUL) { //Green LED only if the ID matched and the command was known new_cmd_led = 1; } } #endif //USE_COMM //END - 10kHz Refresh } else { //Asynchronous code goes here. //WatchDog Clock (Safety-CoP) toggle_wdclk ^= 1; WDCLK_Write(toggle_wdclk); } } }
int main(void) { //Local variables: uint8 i = 0; unsigned char result = 0; uint8 cmd_ready_usb = 0; static uint8 new_cmd_led = 0; uint16 safety_delay = 0; uint8 i2c_time_share = 0; //uint8 toggle_led = 0; //Power on delay with LEDs power_on(); //Initialize all the peripherals init_peripherals(); //Start with an empty buffer flexsea_clear_slave_read_buffer(); //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- //Blocking Test code - enable one and only one for special //debugging. Normal code WILL NOT EXECUTE when this is enabled! //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //strain_test_blocking(); //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //Non-Blocking Test code //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //... //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //Main loop while(1) { if(t1_new_value == 1) { //If the time share slot changed we run the timing FSM. Refer to //timing.xlsx for more details. 't1_new_value' updates at 10kHz, //each slot at 1kHz. t1_new_value = 0; //Timing FSM: switch(t1_time_share) { //Case 0: I2C_0 case 0: i2c_time_share++; i2c_time_share %= 4; #ifdef USE_I2C_0 //Subdivided in 4 slots. switch(i2c_time_share) { //Case 0.0: case 0: break; //Case 0.1: case 1: break; //Case 0.2: case 2: //... break; //Case 0.3: case 3: break; default: break; } #endif //USE_I2C_INT break; //Case 1: I2C_1 case 1: break; //Case 2: case 2: break; //Case 3: case 3: //ToDo do this when new data, not randomly strain_filter(); strain_to_ezi2c(); break; //Case 4: User Interface case 4: //Alive LED //alive_led(); //Done in ISR //UI RGB LED: // ToDo... break; //Case 5: case 5: break; //Case 6: case 6: break; case 7: break; //Case 8: case 8: break; //Case 9: case 9: //1s timebase: if(timebase_1s()) { //Insert code that needs to run every second here //... } break; default: break; } //The code below is executed every 100us, after the previous slot. //Keep it short! //BEGIN - 10kHz Refresh //USB Byte Input #ifdef USE_USB get_usb_data(); if(data_ready_usb) { data_ready_usb = 0; //Got new data in, try to decode cmd_ready_usb = unpack_payload_usb(); } #endif //USE_USB //FlexSEA Network Communication #ifdef USE_COMM /* //Valid communication from RS-485? if(cmd_ready_485 != 0) { cmd_ready_485 = 0; //Cheap trick to get first line //ToDo: support more than 1 for(i = 0; i < PAYLOAD_BUF_LEN; i++) { tmp_rx_command_485[i] = rx_command_485[0][i]; } //payload_parse_str() calls the functions (if valid) result = payload_parse_str(tmp_rx_command_485); //LED: if(result == PARSE_SUCCESSFUL) { //Green LED only if the ID matched and the command was known new_cmd_led = 1; } //Test ToDo remove CyDmaClearPendingDrq(DMA_3_Chan); } //Time to reply - RS-485? if(reply_ready_flag) { //We never replied in the same time slot: if(t1_time_share != reply_ready_timestamp) { rs485_puts(reply_ready_buf, reply_ready_len); reply_ready_flag = 0; } } */ //Valid communication from USB? if(cmd_ready_usb != 0) { cmd_ready_usb = 0; //Cheap trick to get first line //ToDo: support more than 1 for(i = 0; i < PAYLOAD_BUF_LEN; i++) { tmp_rx_command_usb[i] = rx_command_usb[0][i]; } //payload_parse_str() calls the functions (if valid) result = payload_parse_str(tmp_rx_command_usb); //LED: if(result == PARSE_SUCCESSFUL) { //Green LED only if the ID matched and the command was known new_cmd_led = 1; } } #endif //USE_COMM //END - 10kHz Refresh } else { //Asynchronous code goes here. //... } } }