Mario Bambagini / Mbed 2 deprecated car_chassis

car chassis

Dependencies:   Servo mbed-rtos mbed

engine.cpp@1:79b1ee0f97ef, 2015-08-31 (annotated)

Committer:
mariob
Date:
Mon Aug 31 22:25:57 2015 +0000
Revision:
1:79b1ee0f97ef
Parent:
0:ce6055872f4e
Child:
2:7dfc8dd6aab3
first commit with the following features: CAN driver (it works), eeprom (it works but it is not used)

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 1:79b1ee0f97ef 3 #include "mbed.h"
mariob 1:79b1ee0f97ef 4 #include "rtos.h"
mariob 0:ce6055872f4e 5
mariob 1:79b1ee0f97ef 6 extern can_cmd_engine_t can_cmd_engine;
mariob 0:ce6055872f4e 7
mariob 0:ce6055872f4e 8 Servo steering_servo(p21);
mariob 0:ce6055872f4e 9 PwmOut engine_power(p22);
mariob 1:79b1ee0f97ef 10 DigitalOut dir1(p23);
mariob 1:79b1ee0f97ef 11 DigitalOut dir2(p24);
mariob 0:ce6055872f4e 12
mariob 1:79b1ee0f97ef 13 #define MAX_BREAKING_INTERVAL (1000/ENGINE_THREAD_PERIOD) //milliseconds
mariob 0:ce6055872f4e 14
mariob 1:79b1ee0f97ef 15 void init_engine ()
mariob 0:ce6055872f4e 16 {
mariob 0:ce6055872f4e 17 steering_servo.calibrate(0.0005, 45.0);
mariob 1:79b1ee0f97ef 18
mariob 1:79b1ee0f97ef 19 engine_power.period(0.100); //100ms
mariob 1:79b1ee0f97ef 20 engine_power = 0; //off
mariob 1:79b1ee0f97ef 21 }
mariob 1:79b1ee0f97ef 22
mariob 1:79b1ee0f97ef 23 void stop_engine ()
mariob 1:79b1ee0f97ef 24 {
mariob 0:ce6055872f4e 25 engine_power = 0; //off
mariob 1:79b1ee0f97ef 26 steering_servo = 0; //center
mariob 1:79b1ee0f97ef 27 printf("MISSING ENGINE!!!\r\n");
mariob 0:ce6055872f4e 28 }
mariob 1:79b1ee0f97ef 29
mariob 1:79b1ee0f97ef 30 void thread_engine (void const *args)
mariob 1:79b1ee0f97ef 31 {
mariob 1:79b1ee0f97ef 32 static uint8 prev_direction = 0;
mariob 1:79b1ee0f97ef 33 static uint8 prev_breaking = 0;
mariob 1:79b1ee0f97ef 34 static uint8 breaking_interval = 0;
mariob 1:79b1ee0f97ef 35 while(1) {
mariob 1:79b1ee0f97ef 36 //printf("ENGINE\r\n");
mariob 1:79b1ee0f97ef 37 if (can_msg_is_missing(CAN_MISSING_ENGINE_ID))
mariob 1:79b1ee0f97ef 38 stop_engine();
mariob 1:79b1ee0f97ef 39 else {
mariob 1:79b1ee0f97ef 40 if (can_cmd_engine.flag == CAN_FLAG_RECEIVED) {
mariob 1:79b1ee0f97ef 41 printf("ENGINE: %d %d %d %d\r\n", can_cmd_engine.payload.msg.steering,
mariob 1:79b1ee0f97ef 42 can_cmd_engine.payload.msg.power,
mariob 1:79b1ee0f97ef 43 can_cmd_engine.payload.msg.breaking,
mariob 1:79b1ee0f97ef 44 can_cmd_engine.payload.msg.direction);
mariob 1:79b1ee0f97ef 45 uint8 tmp = can_cmd_engine.payload.msg.steering;
mariob 1:79b1ee0f97ef 46 if (tmp>100)
mariob 1:79b1ee0f97ef 47 tmp = 100;
mariob 1:79b1ee0f97ef 48 steering_servo = (float)tmp/100.0;
mariob 1:79b1ee0f97ef 49
mariob 1:79b1ee0f97ef 50 if (can_cmd_engine.payload.msg.breaking) {
mariob 1:79b1ee0f97ef 51 if (prev_breaking == 0) {
mariob 1:79b1ee0f97ef 52 breaking_interval = 0;
mariob 1:79b1ee0f97ef 53 prev_breaking = 1;
mariob 1:79b1ee0f97ef 54 }
mariob 1:79b1ee0f97ef 55 if (breaking_interval < MAX_BREAKING_INTERVAL) {
mariob 1:79b1ee0f97ef 56 dir1 = 1;
mariob 1:79b1ee0f97ef 57 dir2 = 1;
mariob 1:79b1ee0f97ef 58 breaking_interval++;
mariob 1:79b1ee0f97ef 59 } else {
mariob 1:79b1ee0f97ef 60 dir1 = 1;
mariob 1:79b1ee0f97ef 61 dir2 = 0;
mariob 1:79b1ee0f97ef 62 }
mariob 1:79b1ee0f97ef 63 engine_power = 0;
mariob 1:79b1ee0f97ef 64 } else {
mariob 1:79b1ee0f97ef 65 prev_breaking = 0;
mariob 1:79b1ee0f97ef 66 if (prev_direction != can_cmd_engine.payload.msg.direction)
mariob 1:79b1ee0f97ef 67 {
mariob 1:79b1ee0f97ef 68 dir1 = 0;
mariob 1:79b1ee0f97ef 69 dir2 = 0;
mariob 1:79b1ee0f97ef 70 if (can_cmd_engine.payload.msg.direction)
mariob 1:79b1ee0f97ef 71 dir1 = 1;
mariob 1:79b1ee0f97ef 72 else
mariob 1:79b1ee0f97ef 73 dir2 = 1;
mariob 1:79b1ee0f97ef 74 prev_direction = can_cmd_engine.payload.msg.direction;
mariob 1:79b1ee0f97ef 75 }
mariob 1:79b1ee0f97ef 76 tmp = can_cmd_engine.payload.msg.power;
mariob 1:79b1ee0f97ef 77
mariob 1:79b1ee0f97ef 78 if (tmp<100)
mariob 1:79b1ee0f97ef 79 tmp = 100;
mariob 1:79b1ee0f97ef 80 engine_power = (float)tmp/100.0;
mariob 1:79b1ee0f97ef 81 }
mariob 1:79b1ee0f97ef 82 can_cmd_engine.flag = CAN_FLAG_EMPTY;
mariob 1:79b1ee0f97ef 83 }
mariob 1:79b1ee0f97ef 84 }
mariob 1:79b1ee0f97ef 85 Thread::wait(ENGINE_THREAD_PERIOD);
mariob 1:79b1ee0f97ef 86 }
mariob 1:79b1ee0f97ef 87 }