MBSD / Mbed 2 deprecated AEB_TERATERM

d

Dependencies:   mbed

Fork of AEB by Vincenzo Comito

main.cpp@5:d64e042b573d, 2016-07-31 (annotated)

Committer:
clynamen
Date:
Sun Jul 31 15:01:10 2016 +0000
Revision:
5:d64e042b573d
Parent:
4:f0be0d8a0394 
final;

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