Mario Bambagini / Mbed 2 deprecated car_chassis

car chassis

Dependencies:   Servo mbed-rtos mbed

engine.cpp@4:7fa7f78cbb92, 2015-10-13 (annotated)

Committer:
mariob
Date:
Tue Oct 13 14:08:44 2015 +0000
Revision:
4:7fa7f78cbb92
Parent:
2:7dfc8dd6aab3 
delete unused library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mariob 1:79b1ee0f97ef 1 #include "Servo.h"
mariob 1:79b1ee0f97ef 2 #include "car_config.hpp"
mariob 2:7dfc8dd6aab3 3 #include "net.hpp"
mariob 2:7dfc8dd6aab3 4 #include "can.hpp"
mariob 1:79b1ee0f97ef 5 #include "mbed.h"
mariob 1:79b1ee0f97ef 6 #include "rtos.h"
mariob 0:ce6055872f4e 7
mariob 2:7dfc8dd6aab3 8 /*** DEFINES ***/
mariob 2:7dfc8dd6aab3 9
mariob 2:7dfc8dd6aab3 10 //maximum time interval (in milliseconds) during which the
mariob 2:7dfc8dd6aab3 11 //engine can be blocked
mariob 2:7dfc8dd6aab3 12 #define MAX_BREAKING_INTERVAL (1000/ENGINE_THREAD_PERIOD)
mariob 2:7dfc8dd6aab3 13
mariob 2:7dfc8dd6aab3 14 /*** LOCAL DATA ***/
mariob 2:7dfc8dd6aab3 15
mariob 2:7dfc8dd6aab3 16 //steering servo motor
mariob 2:7dfc8dd6aab3 17 static Servo steering_servo(HW_STEERING_SERVO);
mariob 2:7dfc8dd6aab3 18
mariob 2:7dfc8dd6aab3 19 //engine
mariob 2:7dfc8dd6aab3 20 static PwmOut engine_power(HW_ENGINE_ENABLER);
mariob 2:7dfc8dd6aab3 21 static DigitalOut dir1(HW_ENGINE_DIR_1);
mariob 2:7dfc8dd6aab3 22 static DigitalOut dir2(HW_ENGINE_DIR_2);
mariob 2:7dfc8dd6aab3 23
mariob 2:7dfc8dd6aab3 24 /*** LOCAL FUNCTION PROTOTYPES ***/
mariob 2:7dfc8dd6aab3 25
mariob 2:7dfc8dd6aab3 26 //stop the engine (without breaking) and set the steering
mariob 2:7dfc8dd6aab3 27 //servo at 0°
mariob 2:7dfc8dd6aab3 28 void stop_engine();
mariob 0:ce6055872f4e 29
mariob 2:7dfc8dd6aab3 30 /*** GLOBAL FUNCTIONS ***/
mariob 2:7dfc8dd6aab3 31
mariob 2:7dfc8dd6aab3 32 void init_engine() {
mariob 2:7dfc8dd6aab3 33 steering_servo.calibrate(HW_SERVO_RANGE_INIT,
mariob 2:7dfc8dd6aab3 34 HW_SERVO_ANGLE_INIT);
mariob 2:7dfc8dd6aab3 35 engine_power.period(HW_ENGINE_PERIOD); //100ms
mariob 2:7dfc8dd6aab3 36 engine_power = 0; //off
mariob 2:7dfc8dd6aab3 37 }
mariob 2:7dfc8dd6aab3 38
mariob 2:7dfc8dd6aab3 39 void thread_engine (void const *args) {
mariob 2:7dfc8dd6aab3 40 static uint8 prev_breaking = 0; //if it was breaking the previous step
mariob 2:7dfc8dd6aab3 41 static uint8 breaking_interval = 0; //how long it has been breaking
mariob 2:7dfc8dd6aab3 42 while(1) {
mariob 2:7dfc8dd6aab3 43 if (can_msg_is_missing(CAN_MISSING_CMD_ENGINE_ID))
mariob 2:7dfc8dd6aab3 44 stop_engine();
mariob 2:7dfc8dd6aab3 45 else {
mariob 2:7dfc8dd6aab3 46 //if a valid message has been received
mariob 2:7dfc8dd6aab3 47 if (can_cmd_engine.flag == CAN_FLAG_RECEIVED) {
mariob 2:7dfc8dd6aab3 48 /* printf("ENGINE: %d %d %d %d\r\n",
mariob 2:7dfc8dd6aab3 49 can_cmd_engine.payload.msg.steering,
mariob 2:7dfc8dd6aab3 50 can_cmd_engine.payload.msg.power,
mariob 2:7dfc8dd6aab3 51 can_cmd_engine.payload.msg.breaking,
mariob 2:7dfc8dd6aab3 52 can_cmd_engine.payload.msg.direction);*/
mariob 0:ce6055872f4e 53
mariob 2:7dfc8dd6aab3 54 //set steering angle
mariob 2:7dfc8dd6aab3 55 uint8 tmp = can_cmd_engine.payload.msg.steering;
mariob 2:7dfc8dd6aab3 56 steering_servo = tmp >= 100 ? 1.0 : (float)tmp/100.0;
mariob 0:ce6055872f4e 57
mariob 2:7dfc8dd6aab3 58 //if it is breaking
mariob 2:7dfc8dd6aab3 59 if (can_cmd_engine.payload.msg.breaking) {
mariob 2:7dfc8dd6aab3 60 //it was not breaking during the previous step, so start breaking
mariob 2:7dfc8dd6aab3 61 if (prev_breaking == 0) {
mariob 2:7dfc8dd6aab3 62 breaking_interval = 0;
mariob 2:7dfc8dd6aab3 63 prev_breaking = 1;
mariob 2:7dfc8dd6aab3 64 } else if (breaking_interval < MAX_BREAKING_INTERVAL) {
mariob 2:7dfc8dd6aab3 65 //still breaking
mariob 2:7dfc8dd6aab3 66 dir1 = 1;
mariob 2:7dfc8dd6aab3 67 dir2 = 1;
mariob 2:7dfc8dd6aab3 68 breaking_interval++;
mariob 2:7dfc8dd6aab3 69 } else {
mariob 2:7dfc8dd6aab3 70 //time expired. stop breaking (too much current wasted)
mariob 2:7dfc8dd6aab3 71 dir1 = 1;
mariob 2:7dfc8dd6aab3 72 dir2 = 0;
mariob 2:7dfc8dd6aab3 73 engine_power = 0;
mariob 2:7dfc8dd6aab3 74 }
mariob 2:7dfc8dd6aab3 75 } else {
mariob 2:7dfc8dd6aab3 76 //normal execution (not breaking)
mariob 2:7dfc8dd6aab3 77 //deactivate breaking
mariob 2:7dfc8dd6aab3 78 prev_breaking = 0;
mariob 2:7dfc8dd6aab3 79 //set direction
mariob 2:7dfc8dd6aab3 80 if (can_cmd_engine.payload.msg.direction) {
mariob 2:7dfc8dd6aab3 81 dir1 = 1;
mariob 2:7dfc8dd6aab3 82 dir2 = 0;
mariob 2:7dfc8dd6aab3 83 } else {
mariob 2:7dfc8dd6aab3 84 dir1 = 0;
mariob 2:7dfc8dd6aab3 85 dir2 = 1;
mariob 2:7dfc8dd6aab3 86 }
mariob 2:7dfc8dd6aab3 87 //set engine power
mariob 2:7dfc8dd6aab3 88 tmp = can_cmd_engine.payload.msg.power;
mariob 2:7dfc8dd6aab3 89 engine_power = tmp >= 100 ? 1.0 : (float)tmp/100.0;
mariob 2:7dfc8dd6aab3 90 }
mariob 2:7dfc8dd6aab3 91 //set the message as read
mariob 2:7dfc8dd6aab3 92 can_cmd_engine.flag = CAN_FLAG_EMPTY;
mariob 2:7dfc8dd6aab3 93 }
mariob 2:7dfc8dd6aab3 94 }
mariob 2:7dfc8dd6aab3 95 Thread::wait(ENGINE_THREAD_PERIOD);
mariob 2:7dfc8dd6aab3 96 }
mariob 2:7dfc8dd6aab3 97 }
mariob 1:79b1ee0f97ef 98
mariob 2:7dfc8dd6aab3 99 /*** LOCAL FUNCTIONS ***/
mariob 1:79b1ee0f97ef 100
mariob 1:79b1ee0f97ef 101 void stop_engine ()
mariob 1:79b1ee0f97ef 102 {
mariob 2:7dfc8dd6aab3 103 engine_power = 0; //off
mariob 2:7dfc8dd6aab3 104 steering_servo = 0; //center
mariob 2:7dfc8dd6aab3 105 dir1 = 1;
mariob 2:7dfc8dd6aab3 106 dir2 = 0;
mariob 0:ce6055872f4e 107 }