car chassis
Dependencies: Servo mbed-rtos mbed
engine.cpp
- Committer:
- mariob
- Date:
- 2015-10-13
- Revision:
- 4:7fa7f78cbb92
- Parent:
- 2:7dfc8dd6aab3
File content as of revision 4:7fa7f78cbb92:
#include "Servo.h" #include "car_config.hpp" #include "net.hpp" #include "can.hpp" #include "mbed.h" #include "rtos.h" /*** DEFINES ***/ //maximum time interval (in milliseconds) during which the //engine can be blocked #define MAX_BREAKING_INTERVAL (1000/ENGINE_THREAD_PERIOD) /*** LOCAL DATA ***/ //steering servo motor static Servo steering_servo(HW_STEERING_SERVO); //engine static PwmOut engine_power(HW_ENGINE_ENABLER); static DigitalOut dir1(HW_ENGINE_DIR_1); static DigitalOut dir2(HW_ENGINE_DIR_2); /*** LOCAL FUNCTION PROTOTYPES ***/ //stop the engine (without breaking) and set the steering //servo at 0° void stop_engine(); /*** GLOBAL FUNCTIONS ***/ void init_engine() { steering_servo.calibrate(HW_SERVO_RANGE_INIT, HW_SERVO_ANGLE_INIT); engine_power.period(HW_ENGINE_PERIOD); //100ms engine_power = 0; //off } void thread_engine (void const *args) { static uint8 prev_breaking = 0; //if it was breaking the previous step static uint8 breaking_interval = 0; //how long it has been breaking while(1) { if (can_msg_is_missing(CAN_MISSING_CMD_ENGINE_ID)) stop_engine(); else { //if a valid message has been received if (can_cmd_engine.flag == CAN_FLAG_RECEIVED) { /* printf("ENGINE: %d %d %d %d\r\n", can_cmd_engine.payload.msg.steering, can_cmd_engine.payload.msg.power, can_cmd_engine.payload.msg.breaking, can_cmd_engine.payload.msg.direction);*/ //set steering angle uint8 tmp = can_cmd_engine.payload.msg.steering; steering_servo = tmp >= 100 ? 1.0 : (float)tmp/100.0; //if it is breaking if (can_cmd_engine.payload.msg.breaking) { //it was not breaking during the previous step, so start breaking if (prev_breaking == 0) { breaking_interval = 0; prev_breaking = 1; } else if (breaking_interval < MAX_BREAKING_INTERVAL) { //still breaking dir1 = 1; dir2 = 1; breaking_interval++; } else { //time expired. stop breaking (too much current wasted) dir1 = 1; dir2 = 0; engine_power = 0; } } else { //normal execution (not breaking) //deactivate breaking prev_breaking = 0; //set direction if (can_cmd_engine.payload.msg.direction) { dir1 = 1; dir2 = 0; } else { dir1 = 0; dir2 = 1; } //set engine power tmp = can_cmd_engine.payload.msg.power; engine_power = tmp >= 100 ? 1.0 : (float)tmp/100.0; } //set the message as read can_cmd_engine.flag = CAN_FLAG_EMPTY; } } Thread::wait(ENGINE_THREAD_PERIOD); } } /*** LOCAL FUNCTIONS ***/ void stop_engine () { engine_power = 0; //off steering_servo = 0; //center dir1 = 1; dir2 = 0; }