Vincenzo Comito / Mbed 2 deprecated AEB

AEB

Dependencies:   mbed

main.cpp@3:4bb49a5dfa47, 2016-07-30 (annotated)

Committer:
clynamen
Date:
Sat Jul 30 13:58:52 2016 +0000
Revision:
3:4bb49a5dfa47
Parent:
2:5811e080f41d 
m;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
clynamen 0:9d530d56a118 1 #include "mbed.h"
clynamen 0:9d530d56a118 2
clynamen 1:45911e86ffee 3 #include "rtwtypes.h"
clynamen 1:45911e86ffee 4 #include "serialdata.h"
clynamen 2:5811e080f41d 5 #include "CircularBuffer.h"
clynamen 3:4bb49a5dfa47 6 #include "BBSerial.h"
clynamen 3:4bb49a5dfa47 7
clynamen 3:4bb49a5dfa47 8
clynamen 3:4bb49a5dfa47 9 // Comment MASTER or SLAVE
clynamen 3:4bb49a5dfa47 10 #define MASTER
clynamen 3:4bb49a5dfa47 11 //#define SLAVE
clynamen 3:4bb49a5dfa47 12
clynamen 3:4bb49a5dfa47 13 enum color {
clynamen 3:4bb49a5dfa47 14 NONE, RED, GREEN, BLUE, LED_M, LED_S
clynamen 3:4bb49a5dfa47 15 };
clynamen 3:4bb49a5dfa47 16
clynamen 3:4bb49a5dfa47 17 enum {
clynamen 3:4bb49a5dfa47 18 LED_OFF = 1,
clynamen 3:4bb49a5dfa47 19 LED_ON = 0
clynamen 3:4bb49a5dfa47 20 };
clynamen 3:4bb49a5dfa47 21
clynamen 3:4bb49a5dfa47 22
clynamen 3:4bb49a5dfa47 23 struct AEBData{
clynamen 3:4bb49a5dfa47 24 AEBData() : distance(1), fault(0), brake(0) {
clynamen 3:4bb49a5dfa47 25 }
clynamen 3:4bb49a5dfa47 26 float distance;
clynamen 3:4bb49a5dfa47 27 float fault;
clynamen 3:4bb49a5dfa47 28 float brake;
clynamen 3:4bb49a5dfa47 29 };
clynamen 0:9d530d56a118 30
clynamen 1:45911e86ffee 31 DigitalOut red(LED_RED);
clynamen 1:45911e86ffee 32 DigitalOut green(LED_GREEN);
clynamen 1:45911e86ffee 33 DigitalOut blue(LED_BLUE);
clynamen 0:9d530d56a118 34 DigitalOut trigger(D2);
clynamen 0:9d530d56a118 35 InterruptIn echo(D4);
clynamen 0:9d530d56a118 36 Timer t;
clynamen 0:9d530d56a118 37 Ticker scheduler;
clynamen 3:4bb49a5dfa47 38 AEBData aebData;
clynamen 0:9d530d56a118 39
clynamen 3:4bb49a5dfa47 40 #ifdef MASTER
clynamen 3:4bb49a5dfa47 41 AEBData slaveData;
clynamen 3:4bb49a5dfa47 42 #endif
clynamen 3:4bb49a5dfa47 43
clynamen 0:9d530d56a118 44
clynamen 0:9d530d56a118 45 Serial pc(USBTX, USBRX); // tx, rx
clynamen 3:4bb49a5dfa47 46 BBSerial boardSerial;
clynamen 1:45911e86ffee 47
clynamen 1:45911e86ffee 48 #include <stddef.h>
clynamen 1:45911e86ffee 49 #include <stdio.h> /* This ert_main.c example uses printf/fflush */
clynamen 1:45911e86ffee 50 #include "AEB0.h" /* Model's header file */
clynamen 1:45911e86ffee 51 #include "rtwtypes.h"
clynamen 0:9d530d56a118 52
clynamen 0:9d530d56a118 53 /*
clynamen 1:45911e86ffee 54 * Associating rt_OneStep with a real-time clock or interrupt service routine
clynamen 1:45911e86ffee 55 * is what makes the generated code "real-time". The function rt_OneStep is
clynamen 1:45911e86ffee 56 * always associated with the base rate of the model. Subrates are managed
clynamen 1:45911e86ffee 57 * by the base rate from inside the generated code. Enabling/disabling
clynamen 1:45911e86ffee 58 * interrupts and floating point context switches are target specific. This
clynamen 1:45911e86ffee 59 * example code indicates where these should take place relative to executing
clynamen 1:45911e86ffee 60 * the generated code step function. Overrun behavior should be tailored to
clynamen 1:45911e86ffee 61 * your application needs. This example simply sets an error status in the
clynamen 1:45911e86ffee 62 * real-time model and returns from rt_OneStep.
clynamen 1:45911e86ffee 63 */
clynamen 1:45911e86ffee 64 void rt_OneStep(void);
clynamen 1:45911e86ffee 65 void rt_OneStep(void)
clynamen 1:45911e86ffee 66 {
clynamen 1:45911e86ffee 67 static boolean_T OverrunFlag = false;
clynamen 1:45911e86ffee 68
clynamen 1:45911e86ffee 69 /* Disable interrupts here */
clynamen 1:45911e86ffee 70
clynamen 1:45911e86ffee 71 /* Check for overrun */
clynamen 1:45911e86ffee 72 if (OverrunFlag) {
clynamen 1:45911e86ffee 73 rtmSetErrorStatus(AEB0_M, "Overrun");
clynamen 1:45911e86ffee 74 return;
clynamen 1:45911e86ffee 75 }
clynamen 1:45911e86ffee 76
clynamen 1:45911e86ffee 77 OverrunFlag = true;
clynamen 1:45911e86ffee 78
clynamen 1:45911e86ffee 79 /* Save FPU context here (if necessary) */
clynamen 1:45911e86ffee 80 /* Re-enable timer or interrupt here */
clynamen 1:45911e86ffee 81 /* Set model inputs here */
clynamen 1:45911e86ffee 82
clynamen 1:45911e86ffee 83 /* Step the model */
clynamen 1:45911e86ffee 84 AEB0_step();
clynamen 1:45911e86ffee 85
clynamen 1:45911e86ffee 86 /* Get model outputs here */
clynamen 1:45911e86ffee 87
clynamen 1:45911e86ffee 88 /* Indicate task complete */
clynamen 1:45911e86ffee 89 OverrunFlag = false;
clynamen 1:45911e86ffee 90
clynamen 1:45911e86ffee 91 /* Disable interrupts here */
clynamen 1:45911e86ffee 92 /* Restore FPU context here (if necessary) */
clynamen 1:45911e86ffee 93 /* Enable interrupts here */
clynamen 1:45911e86ffee 94 }
clynamen 1:45911e86ffee 95
clynamen 0:9d530d56a118 96
clynamen 3:4bb49a5dfa47 97 void start( void )
clynamen 0:9d530d56a118 98 {
clynamen 0:9d530d56a118 99 t.start();
clynamen 0:9d530d56a118 100 }
clynamen 0:9d530d56a118 101
clynamen 3:4bb49a5dfa47 102 //CircularBuffer distanceCircularBuffer(5);
clynamen 2:5811e080f41d 103
clynamen 0:9d530d56a118 104 void stop( void )
clynamen 0:9d530d56a118 105 {
clynamen 0:9d530d56a118 106 t.stop();
clynamen 2:5811e080f41d 107 float distance_cm = t.read_us() * 343/20000;
clynamen 2:5811e080f41d 108 if(distance_cm > 50) {
clynamen 2:5811e080f41d 109 distance_cm = 50;
clynamen 2:5811e080f41d 110 }
clynamen 2:5811e080f41d 111 //distanceCircularBuffer.push_back(distance_cm);
clynamen 3:4bb49a5dfa47 112 aebData.distance = distance_cm;
clynamen 0:9d530d56a118 113 t.reset();
clynamen 0:9d530d56a118 114 }
clynamen 0:9d530d56a118 115
clynamen 0:9d530d56a118 116
clynamen 1:45911e86ffee 117 extern ExtU_AEB0_T AEB0_U;
clynamen 1:45911e86ffee 118 extern ExtY_AEB0_T AEB0_Y;
clynamen 1:45911e86ffee 119
clynamen 1:45911e86ffee 120
clynamen 0:9d530d56a118 121
clynamen 1:45911e86ffee 122 void setColor(color c) {
clynamen 1:45911e86ffee 123 red = LED_OFF;
clynamen 1:45911e86ffee 124 blue = LED_OFF;
clynamen 1:45911e86ffee 125 green = LED_OFF;
clynamen 2:5811e080f41d 126
clynamen 1:45911e86ffee 127 switch(c) {
clynamen 1:45911e86ffee 128 case NONE:
clynamen 1:45911e86ffee 129 break;
clynamen 1:45911e86ffee 130 case RED:
clynamen 1:45911e86ffee 131 red = LED_ON;
clynamen 1:45911e86ffee 132 break;
clynamen 1:45911e86ffee 133 case BLUE:
clynamen 1:45911e86ffee 134 blue = LED_ON;
clynamen 1:45911e86ffee 135 break;
clynamen 1:45911e86ffee 136 case GREEN:
clynamen 1:45911e86ffee 137 green = LED_ON;
clynamen 3:4bb49a5dfa47 138 case LED_M:
clynamen 3:4bb49a5dfa47 139 green = LED_ON;
clynamen 3:4bb49a5dfa47 140 red = LED_ON;
clynamen 3:4bb49a5dfa47 141 break;
clynamen 3:4bb49a5dfa47 142 case LED_S:
clynamen 3:4bb49a5dfa47 143 blue = LED_ON;
clynamen 3:4bb49a5dfa47 144 red = LED_ON;
clynamen 3:4bb49a5dfa47 145 break;
clynamen 3:4bb49a5dfa47 146 default:
clynamen 1:45911e86ffee 147 break;
clynamen 0:9d530d56a118 148 }
clynamen 0:9d530d56a118 149 }
clynamen 0:9d530d56a118 150
clynamen 2:5811e080f41d 151
clynamen 1:45911e86ffee 152 void do_step( void )
clynamen 1:45911e86ffee 153 {
clynamen 3:4bb49a5dfa47 154 #ifdef MASTER
clynamen 3:4bb49a5dfa47 155 // Get data from simulink
clynamen 3:4bb49a5dfa47 156 float speed = serialRecv();
clynamen 3:4bb49a5dfa47 157 // Forward data to slave
clynamen 3:4bb49a5dfa47 158 // boardSerial.printf("%f %f", aebData.fault, speed);
clynamen 3:4bb49a5dfa47 159 #elif defined SLAVE
clynamen 3:4bb49a5dfa47 160 float speed;
clynamen 3:4bb49a5dfa47 161 float masterFault;
clynamen 3:4bb49a5dfa47 162 if(boardSerial.scanf("%f %f", &masterFault, &speed) != 2) {
clynamen 3:4bb49a5dfa47 163 masterFault = 1;
clynamen 3:4bb49a5dfa47 164 speed = 0;
clynamen 3:4bb49a5dfa47 165 }
clynamen 3:4bb49a5dfa47 166 #endif
clynamen 3:4bb49a5dfa47 167
clynamen 3:4bb49a5dfa47 168
clynamen 3:4bb49a5dfa47 169 //Update AEB data
clynamen 3:4bb49a5dfa47 170 AEB0_U.speed_km_h = speed;
clynamen 3:4bb49a5dfa47 171 AEB0_U.distance_m = aebData.distance;
clynamen 1:45911e86ffee 172
clynamen 1:45911e86ffee 173 rt_OneStep();
clynamen 3:4bb49a5dfa47 174
clynamen 3:4bb49a5dfa47 175 aebData.brake = AEB0_Y.brake;
clynamen 3:4bb49a5dfa47 176 float activateBrake = aebData.brake;
clynamen 3:4bb49a5dfa47 177
clynamen 3:4bb49a5dfa47 178 #ifdef SLAVE
clynamen 3:4bb49a5dfa47 179 activateBrake = activateBrake && masterFault;
clynamen 3:4bb49a5dfa47 180 #endif
clynamen 3:4bb49a5dfa47 181
clynamen 3:4bb49a5dfa47 182 if(activateBrake > 0) {
clynamen 2:5811e080f41d 183 setColor(BLUE);
clynamen 2:5811e080f41d 184 } else {
clynamen 3:4bb49a5dfa47 185 #ifdef MASTER
clynamen 3:4bb49a5dfa47 186 setColor(LED_M);
clynamen 3:4bb49a5dfa47 187 #else
clynamen 3:4bb49a5dfa47 188 setColor(LED_S);
clynamen 3:4bb49a5dfa47 189 #endif
clynamen 3:4bb49a5dfa47 190 }
clynamen 3:4bb49a5dfa47 191
clynamen 3:4bb49a5dfa47 192 aebData.fault = AEB0_Y.fault;
clynamen 3:4bb49a5dfa47 193 if(aebData.fault) {
clynamen 3:4bb49a5dfa47 194 setColor(RED);
clynamen 2:5811e080f41d 195 }
clynamen 2:5811e080f41d 196
clynamen 3:4bb49a5dfa47 197
clynamen 3:4bb49a5dfa47 198 #ifdef MASTER
clynamen 3:4bb49a5dfa47 199 AEBData slaveData;
clynamen 3:4bb49a5dfa47 200 // if(boardSerial.scanf("%f %f %f", slaveData.brake,
clynamen 3:4bb49a5dfa47 201 // slaveData.distance, slaveData.fault) != 3) {
clynamen 3:4bb49a5dfa47 202 // slaveData.brake = 0;
clynamen 3:4bb49a5dfa47 203 // slaveData.distance = 0;
clynamen 3:4bb49a5dfa47 204 // slaveData.fault = 1;
clynamen 3:4bb49a5dfa47 205 // }
clynamen 3:4bb49a5dfa47 206
clynamen 3:4bb49a5dfa47 207 slaveData.brake = slaveData.distance = slaveData.fault = 0;
clynamen 3:4bb49a5dfa47 208
clynamen 3:4bb49a5dfa47 209
clynamen 3:4bb49a5dfa47 210 float data[6] = {aebData.brake, aebData.distance, aebData.fault,
clynamen 3:4bb49a5dfa47 211 slaveData.brake, slaveData.distance, slaveData.fault};
clynamen 3:4bb49a5dfa47 212 serialSendVec(data, 6);
clynamen 3:4bb49a5dfa47 213 #elif defined SLAVE
clynamen 3:4bb49a5dfa47 214 boardSerial.printf("%f %f %f", activateBrake, aebData.distance, aebData.fault);
clynamen 3:4bb49a5dfa47 215 #endif
clynamen 0:9d530d56a118 216 }
clynamen 2:5811e080f41d 217
clynamen 2:5811e080f41d 218
clynamen 0:9d530d56a118 219 int main()
clynamen 0:9d530d56a118 220 {
clynamen 0:9d530d56a118 221
clynamen 1:45911e86ffee 222 AEB0_initialize();
clynamen 0:9d530d56a118 223
clynamen 2:5811e080f41d 224 scheduler.attach( &do_step, 0.1 );
clynamen 2:5811e080f41d 225
clynamen 1:45911e86ffee 226 setColor(NONE);
clynamen 2:5811e080f41d 227
clynamen 0:9d530d56a118 228 t.reset();
clynamen 0:9d530d56a118 229 echo.rise( &start );
clynamen 0:9d530d56a118 230 echo.fall( &stop );
clynamen 1:45911e86ffee 231
clynamen 0:9d530d56a118 232 trigger = 0;
clynamen 0:9d530d56a118 233
clynamen 0:9d530d56a118 234 while (true) {
clynamen 0:9d530d56a118 235 trigger = 1;
clynamen 0:9d530d56a118 236 wait_us( 10 );
clynamen 0:9d530d56a118 237 trigger = 0;
clynamen 0:9d530d56a118 238
clynamen 0:9d530d56a118 239 }
clynamen 1:45911e86ffee 240
clynamen 1:45911e86ffee 241 AEB0_terminate();
clynamen 1:45911e86ffee 242 return 0;
clynamen 0:9d530d56a118 243 }