int main(void) { // TivaC application specific code MAP_FPUEnable(); MAP_FPULazyStackingEnable(); // TivaC system clock configuration. Set to 80MHz. MAP_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN); uint8_t button_debounced_delta; uint8_t button_raw_state; ButtonsInit(); // ROS nodehandle initialization and topic registration nh.initNode(); nh.advertise(button_publisher); while (1) { uint8_t button_debounced_state = ButtonsPoll(&button_debounced_delta, &button_raw_state); // Publish message to be transmitted. button_msg.sw1.data = button_debounced_state & LEFT_BUTTON; button_msg.sw2.data = button_debounced_state & RIGHT_BUTTON; button_publisher.publish(&button_msg); // Handle all communications and callbacks. nh.spinOnce(); // Delay for a bit. nh.getHardware()->delay(100); } }
int32_t main(void) { g_ui32SysClock = ROM_SysCtlClockFreqSet(SYSCTL_USE_PLL | SYSCTL_XTAL_25MHZ | SYSCTL_OSC_MAIN | SYSCTL_CFG_VCO_480, 120000000); // Reset index for FFT data inputIndex = 0; MAP_FPULazyStackingEnable(); MAP_FPUEnable(); // Set up ADC sampling and interrupt configureADC(); while(1); /* main function does not return */ }
int main() { // // Enable lazy stacking for interrupt handlers. This allows floating-point // instructions to be used within interrupt handlers, but at the expense of // extra stack usage. // MAP_FPUEnable(); MAP_FPULazyStackingEnable(); // // Set the clocking to run from the PLL at 50MHZ, change to SYSCTL_SYSDIV_2_5 for 80MHz if neeeded // MAP_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ); MAP_IntMasterDisable(); // // Initialize the SW2 button // MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); // // Unlock PF0 so we can change it to a GPIO input // Once we have enabled (unlocked) the commit register then re-lock it // to prevent further changes. PF0 is muxed with NMI thus a special case. // HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY; HWREG(GPIO_PORTF_BASE + GPIO_O_CR) |= 0x01; HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = 0; MAP_GPIOPinTypeGPIOInput(GPIO_PORTF_BASE, GPIO_PIN_0); MAP_GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_0, GPIO_STRENGTH_2MA , GPIO_PIN_TYPE_STD_WPU); // // Initialize the communication layer with a default UART setting // AbstractComm * comm = NULL; AbstractComm * uart_comm = new UARTComm(); purpinsComm communication(uart_comm); // // Initialize the robot object // purpinsRobot purpins; // // Initialize the EEPROM // purpinsSettings settings; // // Initialize the MPU6050-based IMU // mpudata_t mpu; //unsigned long sample_rate = 10 ; //mpu9150_init(0, sample_rate, 0); memset(&mpu, 0, sizeof(mpudata_t)); // // Get the current processor clock frequency. // ulClockMS = MAP_SysCtlClockGet() / (3 * 1000); //unsigned long loop_delay = (1000 / sample_rate) - 2; // // Configure SysTick to occur 1000 times per second // MAP_SysTickPeriodSet(MAP_SysCtlClockGet() / SYSTICKS_PER_SECOND); MAP_SysTickIntEnable(); MAP_SysTickEnable(); MAP_IntMasterEnable(); // // Load the settings from EEPROM // // Load the robot's ID settings.get(PP_SETTING_ID, &id, sizeof(uint32_t)); // Load the motors' PID gains settings.get(PP_SETTING_LEFT_PID, purpins.leftPIDGains(), sizeof(PIDGains)); settings.get(PP_SETTING_RIGHT_PID, purpins.rightPIDGains(), sizeof(PIDGains)); // Load the communication type uint32_t comm_type; settings.get(PP_SETTING_COMM_TYPE, &comm_type, sizeof(uint32_t)); // Holding SW2 at startup forces USB Comm if(comm_type != PP_COMM_TYPE_USB && MAP_GPIOPinRead(GPIO_PORTF_BASE, GPIO_PIN_0) == 1) { if(comm_type == PP_COMM_TYPE_CC3000) { comm = new WiFiComm(); // Load network and server settings settings.get(PP_SETTING_NETWORK_DATA, (static_cast<WiFiComm*>(comm))->network(), sizeof(Network)); settings.get(PP_SETTING_SERVER_DATA, (static_cast<WiFiComm*>(comm))->server(), sizeof(Server)); // Set WiFiComm as the underlying communication layer communication.setCommLayer(comm); } else if(comm_type == PP_COMM_TYPE_ESP8266) { // TODO } else if(comm_type == PP_COMM_TYPE_XBEE) { // TODO } } // // Communication variables and stuff // uint8_t sensors_pack[SENSORS_PACK_SIZE]; bool send_pack = false; unsigned long sensor_streaming_millis = 0; unsigned long last_millis = millis(); uint8_t data[SERIAL_BUFFER_SIZE]; size_t data_size; uint8_t action; // // Main loop, just managing communications // while(1) { // // Check for a new action // action = communication.getMsg(data, data_size); // If we got an action... if(action > 0) { // Process it!!! if(action == PP_ACTION_GET_VERSION) { uint8_t version = VERSION; communication.sendMsg(PP_ACTION_GET_VERSION, (void*)(&version), 1); } else if(action == PP_ACTION_DRIVE) { RobotSpeed speed; memcpy(&speed, data, sizeof(speed)); purpins.setSpeed(&speed); communication.sendAck(PP_ACTION_DRIVE); } else if(action == PP_ACTION_DRIVE_MOTORS) { MotorSpeeds motor_speeds; memcpy(&motor_speeds, data, sizeof(motor_speeds)); purpins.setMotorSpeeds(&motor_speeds); communication.sendAck(PP_ACTION_DRIVE_MOTORS); } else if(action == PP_ACTION_DRIVE_PWM) { MotorPWMs motor_pwms; memcpy(&motor_pwms, data, sizeof(motor_pwms)); purpins.setPWM(&motor_pwms); communication.sendAck(PP_ACTION_DRIVE_PWM); } else if(action == PP_ACTION_GET_ODOMETRY) { Pose odometry; purpins.getOdometry(&odometry); communication.sendMsg(PP_ACTION_GET_ODOMETRY, (void*)(&odometry), sizeof(odometry)); } else if(action == PP_ACTION_GET_MOTOR_SPEEDS) { MotorSpeeds speed; purpins.getMotorSpeeds(&speed); communication.sendMsg(PP_ACTION_GET_MOTOR_SPEEDS, (void*)(&speed), sizeof(speed)); } else if(action == PP_ACTION_GET_ENCODER_PULSES) { EncoderPulses encoder_pulses; purpins.getEncoderTicks(&encoder_pulses); communication.sendMsg(PP_ACTION_GET_ENCODER_PULSES, (void*)(&encoder_pulses), sizeof(encoder_pulses)); } else if(action == PP_ACTION_GET_IMU) { IMU imu_data; imu_data.orientation_x = mpu.fusedQuat[0]; imu_data.orientation_y = mpu.fusedQuat[1]; imu_data.orientation_z = mpu.fusedQuat[2]; imu_data.orientation_w = mpu.fusedQuat[4]; imu_data.linear_acceleration_x = mpu.calibratedAccel[0]; imu_data.linear_acceleration_y = mpu.calibratedAccel[1]; imu_data.linear_acceleration_z = mpu.calibratedAccel[2]; imu_data.angular_velocity_x = mpu.calibratedMag[0]; imu_data.angular_velocity_y = mpu.calibratedMag[1]; imu_data.angular_velocity_z = mpu.calibratedMag[2]; communication.sendMsg(PP_ACTION_GET_IMU, (void*)(&imu_data), sizeof(imu_data)); } else if(action == PP_ACTION_GET_IR_SENSORS) { // TODO: Add the IR sensors } else if(action == PP_ACTION_GET_GAS_SENSOR) { // TODO: Add the gas sensor } else if(action == PP_ACTION_SET_SENSORS_PACK) { bool ok = true; memset(sensors_pack, 0, SENSORS_PACK_SIZE); if(data_size > SENSORS_PACK_SIZE) { communication.error(PP_ERROR_BUFFER_SIZE); ok = false; break; } for(unsigned int i=0 ; i<data_size ; i++) { if(data[i] < PP_ACTION_GET_ODOMETRY || data[i] > PP_ACTION_GET_GAS_SENSOR) { communication.error(PP_ERROR_SENSOR_UNKNOWN); ok = false; break; } } if(ok) { memcpy(sensors_pack, data, data_size); communication.sendAck(PP_ACTION_SET_SENSORS_PACK); } } else if(action == PP_ACTION_GET_SENSORS_PACK) { send_pack = true; } else if(action == PP_ACTION_SET_SENSOR_STREAMING) { float sensor_streaming_frequency; memcpy(&sensor_streaming_frequency, data, sizeof(sensor_streaming_frequency)); sensor_streaming_millis = 1/sensor_streaming_frequency * 1000; communication.sendAck(PP_ACTION_SET_SENSOR_STREAMING); } else if(action == PP_ACTION_SET_GLOBAL_POSE) { Pose pose; memcpy(&pose, data, sizeof(pose)); purpins.setGlobalPose(&pose); communication.sendAck(PP_ACTION_SET_GLOBAL_POSE); } else if(action == PP_ACTION_SET_NEIGHBORS_POSES) { // TODO: Finish this } else if(action == PP_ACTION_SET_ID) { memcpy(&id, data, sizeof(id)); settings.save(PP_SETTING_ID, data, sizeof(uint32_t)); communication.sendAck(PP_ACTION_SET_ID); } else if(action == PP_ACTION_GET_ID) { communication.sendMsg(PP_ACTION_GET_ID, (void*)(&id), sizeof(id)); } else if(action == PP_ACTION_SET_PID_GAINS) { memcpy(purpins.leftPIDGains(), data, sizeof(PIDGains)); memcpy(purpins.rightPIDGains(), data+sizeof(PIDGains), sizeof(PIDGains)); settings.save(PP_SETTING_LEFT_PID, data, sizeof(PIDGains)); settings.save(PP_SETTING_RIGHT_PID, data+sizeof(PIDGains), sizeof(PIDGains)); communication.sendAck(PP_ACTION_SET_PID_GAINS); } else if(action == PP_ACTION_GET_PID_GAINS) { memcpy(data, purpins.leftPIDGains(), sizeof(PIDGains)); memcpy(data+sizeof(PIDGains), purpins.rightPIDGains(), sizeof(PIDGains)); communication.sendMsg(PP_ACTION_GET_ID, (void*)(data), 2*sizeof(PIDGains)); } else if(action == PP_ACTION_SET_SERVER_DATA) { settings.save(PP_SETTING_SERVER_DATA, data, sizeof(Server)); communication.sendAck(PP_ACTION_SET_SERVER_DATA); } else if(action == PP_ACTION_GET_SERVER_DATA) { settings.get(PP_SETTING_SERVER_DATA, data, sizeof(Server)); communication.sendMsg(PP_ACTION_GET_SERVER_DATA, (void*)(data), sizeof(Server)); } else if(action == PP_ACTION_SET_NETWORK_DATA) { settings.save(PP_SETTING_NETWORK_DATA, data, sizeof(Network)); communication.sendAck(PP_ACTION_SET_NETWORK_DATA); } else if(action == PP_ACTION_GET_NETWORK_DATA) { settings.get(PP_SETTING_NETWORK_DATA, data, sizeof(Network)); communication.sendMsg(PP_ACTION_GET_NETWORK_DATA, (void*)(data), sizeof(Network)); } else if(action == PP_ACTION_SET_COMM_TYPE) { memcpy(&comm_type, data, sizeof(comm_type)); if(comm_type != communication.type()) { if(comm_type == PP_COMM_TYPE_USB) { communication.setCommLayer(uart_comm); if(comm != NULL) delete comm; } if(comm_type == PP_COMM_TYPE_CC3000) { if(comm != NULL) delete comm; comm = new WiFiComm(); // Load network and server settings settings.get(PP_SETTING_NETWORK_DATA, (static_cast<WiFiComm*>(comm))->network(), sizeof(Network)); settings.get(PP_SETTING_SERVER_DATA, (static_cast<WiFiComm*>(comm))->server(), sizeof(Server)); // Set WiFiComm as the underlying communication layer communication.setCommLayer(comm); } else if(comm_type == PP_COMM_TYPE_ESP8266) { // TODO } else if(comm_type == PP_COMM_TYPE_XBEE) { // TODO } else { communication.error(PP_ERROR_COMM_UNKNOWN); } communication.sendAck(PP_ACTION_SET_COMM_TYPE); } } } // if(action > 0) // // Manage sensor streaming // unsigned long current_millis = millis(); if(send_pack || (sensor_streaming_millis > 0 && (current_millis - last_millis) >= sensor_streaming_millis)) { size_t size = 0; for(int i=0 ; i<SENSORS_PACK_SIZE ; i++) { if(sensors_pack[i] == 0) break; if(sensors_pack[i] == PP_ACTION_GET_ODOMETRY) { Pose odometry; purpins.getOdometry(&odometry); memcpy(data+size, &odometry, sizeof(odometry)); size += sizeof(odometry); } else if(sensors_pack[i] == PP_ACTION_GET_MOTOR_SPEEDS) { MotorSpeeds speed; purpins.getMotorSpeeds(&speed); memcpy(data+size, &speed, sizeof(speed)); size += sizeof(speed); } else if(sensors_pack[i] == PP_ACTION_GET_ENCODER_PULSES) { EncoderPulses encoder_pulses; purpins.getEncoderTicks(&encoder_pulses); memcpy(data+size, &encoder_pulses, sizeof(encoder_pulses)); size += sizeof(encoder_pulses); } else if(sensors_pack[i] == PP_ACTION_GET_IMU) { IMU imu_data; imu_data.orientation_x = mpu.fusedQuat[0]; imu_data.orientation_y = mpu.fusedQuat[1]; imu_data.orientation_z = mpu.fusedQuat[2]; imu_data.orientation_w = mpu.fusedQuat[4]; imu_data.linear_acceleration_x = mpu.calibratedAccel[0]; imu_data.linear_acceleration_y = mpu.calibratedAccel[1]; imu_data.linear_acceleration_z = mpu.calibratedAccel[2]; imu_data.angular_velocity_x = mpu.calibratedMag[0]; imu_data.angular_velocity_y = mpu.calibratedMag[1]; imu_data.angular_velocity_z = mpu.calibratedMag[2]; memcpy(data+size, &imu_data, sizeof(imu_data)); size += sizeof(imu_data); } else if(sensors_pack[i] == PP_ACTION_GET_IR_SENSORS) { // TODO: Add the IR sensors } else if(sensors_pack[i] == PP_ACTION_GET_GAS_SENSOR) { // TODO: Add the gas sensor } } communication.sendMsg(PP_ACTION_GET_SENSORS_PACK, (void*)(data), size); last_millis = current_millis; send_pack = false; } // if(send_pack ... } // while(1) return 0; }
int main(){ unsigned long ulClockMS=0; // // Enable lazy stacking for interrupt handlers. This allows floating-point // instructions to be used within interrupt handlers, but at the expense of // extra stack usage. // MAP_FPUEnable(); MAP_FPULazyStackingEnable(); // // Set the clocking to run directly from the crystal. // MAP_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ); MAP_IntMasterDisable(); configureQEI(); configurePWM(); /*leftmotor_pid.setMaxOutput(255); leftmotor_pid.setMinOutput(-255); leftmotor_pid.setGains(12.0,5.0,0.0); leftmotor_pid.setSampleTime(1.0/QEILOOPFREQUENCY);*/ leftmotor_pid.setInputLimits(-100,100); leftmotor_pid.setOutputLimits(-255,255); leftmotor_pid.setBias(0.0); leftmotor_pid.setMode(AUTO_MODE); configureUART(); MAP_IntMasterEnable(); // Get the current processor clock frequency. ulClockMS = MAP_SysCtlClockGet() / (3 * 1000); int char_counter=0; char inputbuffer[10]; while(1){ inputbuffer[char_counter]=UARTgetc(); char_counter++; if (char_counter==3){ inputbuffer[3]=0; goal_vel=atoi(inputbuffer); char_counter=0; } //uint32_t pos = MAP_QEIPositionGet(QEI1_BASE); /*uint32_t vel = MAP_QEIVelocityGet(QEI1_BASE); //UARTprintf("pos : %u \n",pos);QEIDirectionGet(QEI1_BASE)*/ /*UARTprintf("vel : %d pwm %d\n",vel,pwm_value); MAP_SysCtlDelay(ulClockMS*50);*/ } return 0; }
// Initialize FreeRTOS and start the initial set of tasks. int main(void) { // TivaC application specific code MAP_FPUEnable(); MAP_FPULazyStackingEnable(); // Run from the PLL at 120 MHz. MAP_SysCtlClockFreqSet((SYSCTL_XTAL_25MHZ | SYSCTL_OSC_MAIN | SYSCTL_USE_PLL | SYSCTL_CFG_VCO_480), TM4C129FREQ); // Make sure the main oscillator is enabled because this is required by // the PHY. The system must have a 25MHz crystal attached to the OSC // pins. The SYSCTL_MOSC_HIGHFREQ parameter is used when the crystal // frequency is 10MHz or higher. MAP_SysCtlMOSCConfigSet(SYSCTL_MOSC_HIGHFREQ); MAP_IntEnable(FAULT_NMI); MAP_IntEnable(FAULT_MPU); MAP_IntEnable(FAULT_BUS); MAP_IntEnable(FAULT_USAGE); ConfigUART(115200); UARTprintf("\n\nWelcome to the EK-TM4C129XL FreeRTOS Demo!\n"); // ROS nodehandle initialization and topic registration nh.initNode("192.168.1.135"); // Start ROS spin task, responsible for handling callbacks and communications if (spinInitTask(&nh)) { UARTprintf("Couldn't create ROS spin task.\n"); while (1); } else { UARTprintf("Created ROS spin task.\n"); } // Register and init subscribe task if (subscribeInitTask(&nh)) { UARTprintf("Couldn't create subscribe task.\n"); while (1); } else { UARTprintf("Created subscribe task.\n"); } // Register and init publish task if (publishInitTask(&nh)) { UARTprintf("Couldn't create publish task.\n"); while (1); } else { UARTprintf("Created publish task.\n"); } // Start the scheduler. This should not return. UARTprintf("Starting scheduller.\n"); vTaskStartScheduler(); // In case the scheduler returns for some reason, print an error and loop forever. while (1) { UARTprintf("Scheduler returned!\n"); } }
void SetupFPU(void) { MAP_FPUEnable(); MAP_FPULazyStackingEnable(); }