MBSD / Mbed 2 deprecated AEB_TERATERM

d

Dependencies:   mbed

Fork of AEB by Vincenzo Comito

main.cpp@2:5811e080f41d, 2016-07-30 (annotated)

Committer:
clynamen
Date:
Sat Jul 30 08:23:21 2016 +0000
Revision:
2:5811e080f41d
Parent:
1:45911e86ffee
Child:
3:4bb49a5dfa47
ok2;

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 0:9d530d56a118 6
clynamen 1:45911e86ffee 7 DigitalOut red(LED_RED);
clynamen 1:45911e86ffee 8 DigitalOut green(LED_GREEN);
clynamen 1:45911e86ffee 9 DigitalOut blue(LED_BLUE);
clynamen 1:45911e86ffee 10
clynamen 0:9d530d56a118 11 DigitalOut trigger(D2);
clynamen 0:9d530d56a118 12 InterruptIn echo(D4);
clynamen 0:9d530d56a118 13 Timer t;
clynamen 0:9d530d56a118 14 Ticker scheduler;
clynamen 0:9d530d56a118 15
clynamen 1:45911e86ffee 16 volatile float distance;
clynamen 1:45911e86ffee 17 float fault = 0;
clynamen 0:9d530d56a118 18
clynamen 0:9d530d56a118 19 Serial pc(USBTX, USBRX); // tx, rx
clynamen 0:9d530d56a118 20
clynamen 0:9d530d56a118 21
clynamen 0:9d530d56a118 22 //
clynamen 0:9d530d56a118 23 // Copy from ert_main.c
clynamen 0:9d530d56a118 24 //
clynamen 1:45911e86ffee 25
clynamen 1:45911e86ffee 26 #include <stddef.h>
clynamen 1:45911e86ffee 27 #include <stdio.h> /* This ert_main.c example uses printf/fflush */
clynamen 1:45911e86ffee 28 #include "AEB0.h" /* Model's header file */
clynamen 1:45911e86ffee 29 #include "rtwtypes.h"
clynamen 0:9d530d56a118 30
clynamen 0:9d530d56a118 31 /*
clynamen 1:45911e86ffee 32 * Associating rt_OneStep with a real-time clock or interrupt service routine
clynamen 1:45911e86ffee 33 * is what makes the generated code "real-time". The function rt_OneStep is
clynamen 1:45911e86ffee 34 * always associated with the base rate of the model. Subrates are managed
clynamen 1:45911e86ffee 35 * by the base rate from inside the generated code. Enabling/disabling
clynamen 1:45911e86ffee 36 * interrupts and floating point context switches are target specific. This
clynamen 1:45911e86ffee 37 * example code indicates where these should take place relative to executing
clynamen 1:45911e86ffee 38 * the generated code step function. Overrun behavior should be tailored to
clynamen 1:45911e86ffee 39 * your application needs. This example simply sets an error status in the
clynamen 1:45911e86ffee 40 * real-time model and returns from rt_OneStep.
clynamen 1:45911e86ffee 41 */
clynamen 1:45911e86ffee 42 void rt_OneStep(void);
clynamen 1:45911e86ffee 43 void rt_OneStep(void)
clynamen 1:45911e86ffee 44 {
clynamen 1:45911e86ffee 45 static boolean_T OverrunFlag = false;
clynamen 1:45911e86ffee 46
clynamen 1:45911e86ffee 47 /* Disable interrupts here */
clynamen 1:45911e86ffee 48
clynamen 1:45911e86ffee 49 /* Check for overrun */
clynamen 1:45911e86ffee 50 if (OverrunFlag) {
clynamen 1:45911e86ffee 51 rtmSetErrorStatus(AEB0_M, "Overrun");
clynamen 1:45911e86ffee 52 return;
clynamen 1:45911e86ffee 53 }
clynamen 1:45911e86ffee 54
clynamen 1:45911e86ffee 55 OverrunFlag = true;
clynamen 1:45911e86ffee 56
clynamen 1:45911e86ffee 57 /* Save FPU context here (if necessary) */
clynamen 1:45911e86ffee 58 /* Re-enable timer or interrupt here */
clynamen 1:45911e86ffee 59 /* Set model inputs here */
clynamen 1:45911e86ffee 60
clynamen 1:45911e86ffee 61 /* Step the model */
clynamen 1:45911e86ffee 62 AEB0_step();
clynamen 1:45911e86ffee 63
clynamen 1:45911e86ffee 64 /* Get model outputs here */
clynamen 1:45911e86ffee 65
clynamen 1:45911e86ffee 66 /* Indicate task complete */
clynamen 1:45911e86ffee 67 OverrunFlag = false;
clynamen 1:45911e86ffee 68
clynamen 1:45911e86ffee 69 /* Disable interrupts here */
clynamen 1:45911e86ffee 70 /* Restore FPU context here (if necessary) */
clynamen 1:45911e86ffee 71 /* Enable interrupts here */
clynamen 1:45911e86ffee 72 }
clynamen 1:45911e86ffee 73
clynamen 0:9d530d56a118 74
clynamen 0:9d530d56a118 75 //
clynamen 0:9d530d56a118 76 // End copy
clynamen 0:9d530d56a118 77 //
clynamen 0:9d530d56a118 78
clynamen 1:45911e86ffee 79
clynamen 0:9d530d56a118 80
clynamen 0:9d530d56a118 81 void start( void )
clynamen 0:9d530d56a118 82 {
clynamen 0:9d530d56a118 83 t.start();
clynamen 0:9d530d56a118 84 }
clynamen 0:9d530d56a118 85
clynamen 2:5811e080f41d 86 CircularBuffer distanceCircularBuffer(5);
clynamen 2:5811e080f41d 87
clynamen 2:5811e080f41d 88 float sensDist = 1;
clynamen 2:5811e080f41d 89
clynamen 0:9d530d56a118 90 void stop( void )
clynamen 0:9d530d56a118 91 {
clynamen 0:9d530d56a118 92 t.stop();
clynamen 2:5811e080f41d 93 float distance_cm = t.read_us() * 343/20000;
clynamen 2:5811e080f41d 94 if(distance_cm > 50) {
clynamen 2:5811e080f41d 95 distance_cm = 50;
clynamen 2:5811e080f41d 96 }
clynamen 2:5811e080f41d 97 //distanceCircularBuffer.push_back(distance_cm);
clynamen 2:5811e080f41d 98 sensDist = distance_cm;
clynamen 0:9d530d56a118 99 t.reset();
clynamen 0:9d530d56a118 100 }
clynamen 0:9d530d56a118 101
clynamen 0:9d530d56a118 102
clynamen 1:45911e86ffee 103 extern ExtU_AEB0_T AEB0_U;
clynamen 1:45911e86ffee 104 extern ExtY_AEB0_T AEB0_Y;
clynamen 1:45911e86ffee 105
clynamen 1:45911e86ffee 106 enum color {
clynamen 1:45911e86ffee 107 NONE, RED, GREEN, BLUE
clynamen 1:45911e86ffee 108 };
clynamen 1:45911e86ffee 109
clynamen 1:45911e86ffee 110 enum {
clynamen 2:5811e080f41d 111 LED_OFF = 1,
clynamen 2:5811e080f41d 112 LED_ON = 0
clynamen 1:45911e86ffee 113 };
clynamen 0:9d530d56a118 114
clynamen 1:45911e86ffee 115 void setColor(color c) {
clynamen 1:45911e86ffee 116 red = LED_OFF;
clynamen 1:45911e86ffee 117 blue = LED_OFF;
clynamen 1:45911e86ffee 118 green = LED_OFF;
clynamen 2:5811e080f41d 119
clynamen 1:45911e86ffee 120 switch(c) {
clynamen 1:45911e86ffee 121 case NONE:
clynamen 1:45911e86ffee 122 break;
clynamen 1:45911e86ffee 123 case RED:
clynamen 1:45911e86ffee 124 red = LED_ON;
clynamen 1:45911e86ffee 125 break;
clynamen 1:45911e86ffee 126 case BLUE:
clynamen 1:45911e86ffee 127 blue = LED_ON;
clynamen 1:45911e86ffee 128 break;
clynamen 1:45911e86ffee 129 case GREEN:
clynamen 1:45911e86ffee 130 green = LED_ON;
clynamen 1:45911e86ffee 131 break;
clynamen 0:9d530d56a118 132 }
clynamen 0:9d530d56a118 133 }
clynamen 0:9d530d56a118 134
clynamen 2:5811e080f41d 135
clynamen 2:5811e080f41d 136
clynamen 2:5811e080f41d 137 float brake;
clynamen 2:5811e080f41d 138
clynamen 2:5811e080f41d 139
clynamen 1:45911e86ffee 140 void do_step( void )
clynamen 1:45911e86ffee 141 {
clynamen 2:5811e080f41d 142 AEB0_U.speed_km_h = serialRecv();
clynamen 2:5811e080f41d 143 AEB0_U.distance_m = sensDist;
clynamen 1:45911e86ffee 144
clynamen 1:45911e86ffee 145 rt_OneStep();
clynamen 2:5811e080f41d 146 brake = AEB0_Y.brake;
clynamen 2:5811e080f41d 147 if(brake > 0) {
clynamen 2:5811e080f41d 148 setColor(BLUE);
clynamen 2:5811e080f41d 149 } else {
clynamen 2:5811e080f41d 150 setColor(NONE);
clynamen 2:5811e080f41d 151 }
clynamen 2:5811e080f41d 152
clynamen 1:45911e86ffee 153 fault = AEB0_Y.fault;
clynamen 1:45911e86ffee 154 if(fault) {
clynamen 1:45911e86ffee 155 setColor(RED);
clynamen 0:9d530d56a118 156 }
clynamen 2:5811e080f41d 157 float data[3] = {brake, sensDist, fault};
clynamen 1:45911e86ffee 158 serialSendVec(data, 3);
clynamen 0:9d530d56a118 159 }
clynamen 2:5811e080f41d 160
clynamen 2:5811e080f41d 161
clynamen 0:9d530d56a118 162 int main()
clynamen 0:9d530d56a118 163 {
clynamen 0:9d530d56a118 164
clynamen 1:45911e86ffee 165 AEB0_initialize();
clynamen 0:9d530d56a118 166
clynamen 2:5811e080f41d 167 scheduler.attach( &do_step, 0.1 );
clynamen 2:5811e080f41d 168
clynamen 1:45911e86ffee 169 setColor(NONE);
clynamen 2:5811e080f41d 170
clynamen 0:9d530d56a118 171 t.reset();
clynamen 0:9d530d56a118 172 echo.rise( &start );
clynamen 0:9d530d56a118 173 echo.fall( &stop );
clynamen 1:45911e86ffee 174
clynamen 0:9d530d56a118 175 trigger = 0;
clynamen 0:9d530d56a118 176
clynamen 0:9d530d56a118 177 while (true) {
clynamen 1:45911e86ffee 178 // pc.printf( "Reading inputs....\n\r" );
clynamen 1:45911e86ffee 179
clynamen 0:9d530d56a118 180
clynamen 0:9d530d56a118 181 trigger = 1;
clynamen 0:9d530d56a118 182 wait_us( 10 );
clynamen 0:9d530d56a118 183 trigger = 0;
clynamen 0:9d530d56a118 184
clynamen 0:9d530d56a118 185
clynamen 0:9d530d56a118 186 }
clynamen 1:45911e86ffee 187
clynamen 1:45911e86ffee 188 AEB0_terminate();
clynamen 1:45911e86ffee 189 return 0;
clynamen 0:9d530d56a118 190 }