MBSD_assignment / Mbed 2 deprecated AEB

Program that execute AEB system

Dependencies:   AEB Ultrasonic Controller_Master mbed

Dependents:   AEB

main.cpp@2:fb694fb2ef9b, 2016-07-06 (annotated)

Committer:
AndreaAndreoli
Date:
Wed Jul 06 09:17:23 2016 +0000
Revision:
2:fb694fb2ef9b
Parent:
1:8cb509a319e5
Child:
3:06075cbd49bd
Child:
6:d0717be58ca3
QA function added;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AndreaAndreoli 2:fb694fb2ef9b 1 #include "mbed.h"
AndreaAndreoli 2:fb694fb2ef9b 2 #include "Ultrasonic.h"
AndreaAndreoli 2:fb694fb2ef9b 3
AndreaAndreoli 2:fb694fb2ef9b 4 extern "C" {
AndreaAndreoli 2:fb694fb2ef9b 5 #include "Controller_Master.h" /* Model's header file */
AndreaAndreoli 2:fb694fb2ef9b 6 #include "rtwtypes.h"
AndreaAndreoli 2:fb694fb2ef9b 7 }
AndreaAndreoli 2:fb694fb2ef9b 8
AndreaAndreoli 2:fb694fb2ef9b 9 static RT_MODEL_Controller_Master_T Controller_Master_M_;
AndreaAndreoli 2:fb694fb2ef9b 10 static RT_MODEL_Controller_Master_T *const Controller_Master_M =
AndreaAndreoli 2:fb694fb2ef9b 11 &Controller_Master_M_; /* Real-time model */
AndreaAndreoli 2:fb694fb2ef9b 12 static B_Controller_Master_T Controller_Master_B;/* Observable signals */
AndreaAndreoli 2:fb694fb2ef9b 13 static DW_Controller_Master_T Controller_Master_DW;/* Observable states */
AndreaAndreoli 2:fb694fb2ef9b 14
AndreaAndreoli 2:fb694fb2ef9b 15 /* '<Root>/V' */
AndreaAndreoli 2:fb694fb2ef9b 16 static real_T Controller_Master_U_V;
AndreaAndreoli 2:fb694fb2ef9b 17
AndreaAndreoli 2:fb694fb2ef9b 18 /* '<Root>/D_M' */
AndreaAndreoli 2:fb694fb2ef9b 19 static real_T Controller_Master_U_D_M;
AndreaAndreoli 2:fb694fb2ef9b 20
AndreaAndreoli 2:fb694fb2ef9b 21 /* '<Root>/SLAVE' */
AndreaAndreoli 2:fb694fb2ef9b 22 static uint8_T Controller_Master_U_Slave;
AndreaAndreoli 2:fb694fb2ef9b 23
AndreaAndreoli 2:fb694fb2ef9b 24 /* '<Root>/QA_EN' */
AndreaAndreoli 2:fb694fb2ef9b 25 static boolean_T Controller_Master_U_QA_EN;
AndreaAndreoli 2:fb694fb2ef9b 26
AndreaAndreoli 2:fb694fb2ef9b 27 /* '<Root>/BRAKE' */
AndreaAndreoli 2:fb694fb2ef9b 28 static uint8_T Controller_Master_Y_BRAKE;
AndreaAndreoli 2:fb694fb2ef9b 29
AndreaAndreoli 2:fb694fb2ef9b 30 /* '<Root>/ACC' */
AndreaAndreoli 2:fb694fb2ef9b 31 static uint8_T Controller_Master_Y_ACC;
AndreaAndreoli 2:fb694fb2ef9b 32
AndreaAndreoli 2:fb694fb2ef9b 33 /* '<Root>/LED_RED' */
AndreaAndreoli 2:fb694fb2ef9b 34 static uint8_T Controller_Master_Y_LED_RED;
AndreaAndreoli 2:fb694fb2ef9b 35
AndreaAndreoli 2:fb694fb2ef9b 36 /* '<Root>/LED_GREEN' */
AndreaAndreoli 2:fb694fb2ef9b 37 static uint8_T Controller_Master_Y_LED_GREEN;
AndreaAndreoli 2:fb694fb2ef9b 38
AndreaAndreoli 2:fb694fb2ef9b 39 /* '<Root>/LED_BLUE' */
AndreaAndreoli 2:fb694fb2ef9b 40 static uint8_T Controller_Master_Y_LED_BLUE;
AndreaAndreoli 2:fb694fb2ef9b 41
AndreaAndreoli 2:fb694fb2ef9b 42 /* '<Root>/MASTER' */
AndreaAndreoli 2:fb694fb2ef9b 43 static uint8_T Controller_Master_Y_MASTER;
AndreaAndreoli 2:fb694fb2ef9b 44
AndreaAndreoli 2:fb694fb2ef9b 45 /*
AndreaAndreoli 2:fb694fb2ef9b 46 * Associating rt_OneStep with a real-time clock or interrupt service routine
AndreaAndreoli 2:fb694fb2ef9b 47 * is what makes the generated code "real-time". The function rt_OneStep is
AndreaAndreoli 2:fb694fb2ef9b 48 * always associated with the base rate of the model. Subrates are managed
AndreaAndreoli 2:fb694fb2ef9b 49 * by the base rate from inside the generated code. Enabling/disabling
AndreaAndreoli 2:fb694fb2ef9b 50 * interrupts and floating point context switches are target specific. This
AndreaAndreoli 2:fb694fb2ef9b 51 * example code indicates where these should take place relative to executing
AndreaAndreoli 2:fb694fb2ef9b 52 * the generated code step function. Overrun behavior should be tailored to
AndreaAndreoli 2:fb694fb2ef9b 53 * your application needs. This example simply sets an error status in the
AndreaAndreoli 2:fb694fb2ef9b 54 * real-time model and returns from rt_OneStep.
AndreaAndreoli 2:fb694fb2ef9b 55 */
AndreaAndreoli 2:fb694fb2ef9b 56 void rt_OneStep(RT_MODEL_Controller_Master_T *const Controller_Master_M);
AndreaAndreoli 2:fb694fb2ef9b 57 void step();
AndreaAndreoli 2:fb694fb2ef9b 58
AndreaAndreoli 2:fb694fb2ef9b 59
AndreaAndreoli 2:fb694fb2ef9b 60 Ultrasonic sonic(D2,D4); // Just call this funtion to initialize the ultrasonic sensor
AndreaAndreoli 2:fb694fb2ef9b 61 // D2 trigger D4 echo
AndreaAndreoli 2:fb694fb2ef9b 62
AndreaAndreoli 2:fb694fb2ef9b 63 DigitalOut led_R(LED_RED);
AndreaAndreoli 2:fb694fb2ef9b 64 DigitalOut led_G(LED_GREEN);
AndreaAndreoli 2:fb694fb2ef9b 65 DigitalOut led_B(LED_BLUE);
AndreaAndreoli 2:fb694fb2ef9b 66 DigitalOut master_out(D8);
AndreaAndreoli 2:fb694fb2ef9b 67 DigitalIn slave(D6);
AndreaAndreoli 2:fb694fb2ef9b 68 float V = 29; // Set the vehicle speed here
AndreaAndreoli 2:fb694fb2ef9b 69 Ticker t;
AndreaAndreoli 2:fb694fb2ef9b 70 Serial pc(USBTX, USBRX); // tx, rx
AndreaAndreoli 2:fb694fb2ef9b 71
AndreaAndreoli 2:fb694fb2ef9b 72 int main()
AndreaAndreoli 2:fb694fb2ef9b 73 {
AndreaAndreoli 2:fb694fb2ef9b 74 /* Pack model data into RTM */
AndreaAndreoli 2:fb694fb2ef9b 75 Controller_Master_M->ModelData.blockIO = &Controller_Master_B;
AndreaAndreoli 2:fb694fb2ef9b 76 Controller_Master_M->ModelData.dwork = &Controller_Master_DW;
AndreaAndreoli 2:fb694fb2ef9b 77
AndreaAndreoli 2:fb694fb2ef9b 78
AndreaAndreoli 2:fb694fb2ef9b 79 Controller_Master_U_V = V;
AndreaAndreoli 2:fb694fb2ef9b 80 Controller_Master_U_D_M = 50;
AndreaAndreoli 2:fb694fb2ef9b 81 Controller_Master_U_Slave = 1;
AndreaAndreoli 2:fb694fb2ef9b 82 Controller_Master_U_QA_EN = true;
AndreaAndreoli 2:fb694fb2ef9b 83
AndreaAndreoli 2:fb694fb2ef9b 84 /* Initialize model */
AndreaAndreoli 2:fb694fb2ef9b 85 Controller_Master_initialize(Controller_Master_M, &Controller_Master_U_V,
AndreaAndreoli 2:fb694fb2ef9b 86 &Controller_Master_U_D_M, &Controller_Master_U_Slave,
AndreaAndreoli 2:fb694fb2ef9b 87 &Controller_Master_U_QA_EN, &Controller_Master_Y_BRAKE,
AndreaAndreoli 2:fb694fb2ef9b 88 &Controller_Master_Y_ACC, &Controller_Master_Y_LED_RED,
AndreaAndreoli 2:fb694fb2ef9b 89 &Controller_Master_Y_LED_GREEN, &Controller_Master_Y_LED_BLUE,
AndreaAndreoli 2:fb694fb2ef9b 90 &Controller_Master_Y_MASTER);
AndreaAndreoli 2:fb694fb2ef9b 91
AndreaAndreoli 2:fb694fb2ef9b 92 /* Attach rt_OneStep to a timer or interrupt service routine with
AndreaAndreoli 2:fb694fb2ef9b 93 * period 0.1 seconds (the model's base sample time) here. The
AndreaAndreoli 2:fb694fb2ef9b 94 * call syntax for rt_OneStep is
AndreaAndreoli 2:fb694fb2ef9b 95 *
AndreaAndreoli 2:fb694fb2ef9b 96 * rt_OneStep(Controller_Master_M);
AndreaAndreoli 2:fb694fb2ef9b 97 */
AndreaAndreoli 2:fb694fb2ef9b 98 t.attach(&step,0.1);
AndreaAndreoli 2:fb694fb2ef9b 99 led_B.write(1);
AndreaAndreoli 2:fb694fb2ef9b 100 led_G.write(1);
AndreaAndreoli 2:fb694fb2ef9b 101 led_R.write(1);
AndreaAndreoli 2:fb694fb2ef9b 102 slave.mode(PullDown);
AndreaAndreoli 2:fb694fb2ef9b 103 master_out.write(1);
AndreaAndreoli 2:fb694fb2ef9b 104 while (true) {
AndreaAndreoli 2:fb694fb2ef9b 105 wait(0.2);
AndreaAndreoli 2:fb694fb2ef9b 106 //led_R= !led_R;
AndreaAndreoli 2:fb694fb2ef9b 107
AndreaAndreoli 2:fb694fb2ef9b 108 }
AndreaAndreoli 2:fb694fb2ef9b 109
AndreaAndreoli 2:fb694fb2ef9b 110 /* Disable rt_OneStep() here */
AndreaAndreoli 2:fb694fb2ef9b 111
AndreaAndreoli 2:fb694fb2ef9b 112 /* Terminate model */
AndreaAndreoli 2:fb694fb2ef9b 113 Controller_Master_terminate(Controller_Master_M);
AndreaAndreoli 2:fb694fb2ef9b 114 }
AndreaAndreoli 2:fb694fb2ef9b 115
AndreaAndreoli 2:fb694fb2ef9b 116 void step()
AndreaAndreoli 2:fb694fb2ef9b 117 {
AndreaAndreoli 2:fb694fb2ef9b 118 Controller_Master_U_V = V;
AndreaAndreoli 2:fb694fb2ef9b 119 Controller_Master_U_D_M = sonic.read_cm();
AndreaAndreoli 2:fb694fb2ef9b 120 Controller_Master_U_Slave = slave.read();
AndreaAndreoli 2:fb694fb2ef9b 121 Controller_Master_U_QA_EN = true;
AndreaAndreoli 2:fb694fb2ef9b 122 rt_OneStep(Controller_Master_M);
AndreaAndreoli 2:fb694fb2ef9b 123 master_out.write(Controller_Master_Y_MASTER);
AndreaAndreoli 2:fb694fb2ef9b 124 pc.printf("blue: %d \n", Controller_Master_Y_LED_BLUE); // Call read_cm() to get the distance in cm
AndreaAndreoli 2:fb694fb2ef9b 125 pc.printf("red: %d \n", Controller_Master_Y_LED_RED);
AndreaAndreoli 2:fb694fb2ef9b 126 pc.printf("master: %d \n", Controller_Master_Y_MASTER);
AndreaAndreoli 2:fb694fb2ef9b 127 pc.printf("brake: %d \n", Controller_Master_Y_BRAKE);
AndreaAndreoli 2:fb694fb2ef9b 128 pc.printf("green: %d \n", Controller_Master_Y_LED_GREEN);
AndreaAndreoli 2:fb694fb2ef9b 129 pc.printf("acc: %d \n", Controller_Master_Y_ACC);
AndreaAndreoli 2:fb694fb2ef9b 130 led_B = !Controller_Master_Y_LED_BLUE; // negate because 1 -> led off
AndreaAndreoli 2:fb694fb2ef9b 131 led_R = !Controller_Master_Y_LED_RED;
AndreaAndreoli 2:fb694fb2ef9b 132 led_G = !Controller_Master_Y_LED_GREEN;
AndreaAndreoli 2:fb694fb2ef9b 133 }
AndreaAndreoli 2:fb694fb2ef9b 134
AndreaAndreoli 2:fb694fb2ef9b 135 void rt_OneStep(RT_MODEL_Controller_Master_T *const Controller_Master_M)
AndreaAndreoli 2:fb694fb2ef9b 136 {
AndreaAndreoli 2:fb694fb2ef9b 137 static boolean_T OverrunFlag = false;
AndreaAndreoli 2:fb694fb2ef9b 138
AndreaAndreoli 2:fb694fb2ef9b 139 /* Disable interrupts here */
AndreaAndreoli 2:fb694fb2ef9b 140
AndreaAndreoli 2:fb694fb2ef9b 141 /* Check for overrun */
AndreaAndreoli 2:fb694fb2ef9b 142 if (OverrunFlag) {
AndreaAndreoli 2:fb694fb2ef9b 143 rtmSetErrorStatus(Controller_Master_M, "Overrun");
AndreaAndreoli 2:fb694fb2ef9b 144 return;
AndreaAndreoli 2:fb694fb2ef9b 145 }
AndreaAndreoli 2:fb694fb2ef9b 146
AndreaAndreoli 2:fb694fb2ef9b 147 OverrunFlag = true;
AndreaAndreoli 2:fb694fb2ef9b 148
AndreaAndreoli 2:fb694fb2ef9b 149 /* Save FPU context here (if necessary) */
AndreaAndreoli 2:fb694fb2ef9b 150 /* Re-enable timer or interrupt here */
AndreaAndreoli 2:fb694fb2ef9b 151 /* Set model inputs here */
AndreaAndreoli 2:fb694fb2ef9b 152
AndreaAndreoli 2:fb694fb2ef9b 153 /* Step the model */
AndreaAndreoli 2:fb694fb2ef9b 154 Controller_Master_step(Controller_Master_M, Controller_Master_U_V,
AndreaAndreoli 2:fb694fb2ef9b 155 Controller_Master_U_D_M, Controller_Master_U_QA_EN,
AndreaAndreoli 2:fb694fb2ef9b 156 &Controller_Master_Y_BRAKE, &Controller_Master_Y_ACC,
AndreaAndreoli 2:fb694fb2ef9b 157 &Controller_Master_Y_LED_RED, &Controller_Master_Y_LED_GREEN,
AndreaAndreoli 2:fb694fb2ef9b 158 &Controller_Master_Y_LED_BLUE, &Controller_Master_Y_MASTER);
AndreaAndreoli 2:fb694fb2ef9b 159
AndreaAndreoli 2:fb694fb2ef9b 160 /* Get model outputs here */
AndreaAndreoli 2:fb694fb2ef9b 161
AndreaAndreoli 2:fb694fb2ef9b 162 /* Indicate task complete */
AndreaAndreoli 2:fb694fb2ef9b 163 OverrunFlag = false;
AndreaAndreoli 2:fb694fb2ef9b 164
AndreaAndreoli 2:fb694fb2ef9b 165 /* Disable interrupts here */
AndreaAndreoli 2:fb694fb2ef9b 166 /* Restore FPU context here (if necessary) */
AndreaAndreoli 2:fb694fb2ef9b 167 /* Enable interrupts here */
AndreaAndreoli 2:fb694fb2ef9b 168 }