MBSD_assignment / Mbed 2 deprecated AEB

Program that execute AEB system

Dependencies:   AEB Ultrasonic Controller_Master mbed

Dependents:   AEB

main.cpp@3:06075cbd49bd, 2016-07-06 (annotated)

Committer:
AndreaAndreoli
Date:
Wed Jul 06 10:57:35 2016 +0000
Revision:
3:06075cbd49bd
Parent:
2:fb694fb2ef9b
Child:
4:0df8a61cbee9
first commit;

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 3:06075cbd49bd 5 #include "Controller_Slave.h" /* Model's header file */
AndreaAndreoli 2:fb694fb2ef9b 6 #include "rtwtypes.h"
AndreaAndreoli 2:fb694fb2ef9b 7 }
AndreaAndreoli 2:fb694fb2ef9b 8
AndreaAndreoli 3:06075cbd49bd 9 static RT_MODEL_Controller_Slave_T Controller_Slave_M_;
AndreaAndreoli 3:06075cbd49bd 10 static RT_MODEL_Controller_Slave_T *const Controller_Slave_M =
AndreaAndreoli 3:06075cbd49bd 11 &Controller_Slave_M_; /* Real-time model */
AndreaAndreoli 3:06075cbd49bd 12 static B_Controller_Slave_T Controller_Slave_B;/* Observable signals */
AndreaAndreoli 3:06075cbd49bd 13 static DW_Controller_Slave_T Controller_Slave_DW;/* Observable states */
AndreaAndreoli 2:fb694fb2ef9b 14
AndreaAndreoli 2:fb694fb2ef9b 15 /* '<Root>/V' */
AndreaAndreoli 3:06075cbd49bd 16 static real_T Controller_Slave_U_V;
AndreaAndreoli 2:fb694fb2ef9b 17
AndreaAndreoli 3:06075cbd49bd 18 /* '<Root>/D_S' */
AndreaAndreoli 3:06075cbd49bd 19 static real_T Controller_Slave_U_D_S;
AndreaAndreoli 2:fb694fb2ef9b 20
AndreaAndreoli 3:06075cbd49bd 21 /* '<Root>/MASTER' */
AndreaAndreoli 3:06075cbd49bd 22 static uint8_T Controller_Slave_U_Master;
AndreaAndreoli 2:fb694fb2ef9b 23
AndreaAndreoli 2:fb694fb2ef9b 24 /* '<Root>/QA_EN' */
AndreaAndreoli 3:06075cbd49bd 25 static boolean_T Controller_Slave_U_QA_EN;
AndreaAndreoli 2:fb694fb2ef9b 26
AndreaAndreoli 2:fb694fb2ef9b 27 /* '<Root>/BRAKE' */
AndreaAndreoli 3:06075cbd49bd 28 static uint8_T Controller_Slave_Y_BRAKE;
AndreaAndreoli 2:fb694fb2ef9b 29
AndreaAndreoli 2:fb694fb2ef9b 30 /* '<Root>/ACC' */
AndreaAndreoli 3:06075cbd49bd 31 static uint8_T Controller_Slave_Y_ACC;
AndreaAndreoli 2:fb694fb2ef9b 32
AndreaAndreoli 2:fb694fb2ef9b 33 /* '<Root>/LED_RED' */
AndreaAndreoli 3:06075cbd49bd 34 static uint8_T Controller_Slave_Y_LED_RED;
AndreaAndreoli 2:fb694fb2ef9b 35
AndreaAndreoli 2:fb694fb2ef9b 36 /* '<Root>/LED_GREEN' */
AndreaAndreoli 3:06075cbd49bd 37 static uint8_T Controller_Slave_Y_LED_GREEN;
AndreaAndreoli 2:fb694fb2ef9b 38
AndreaAndreoli 2:fb694fb2ef9b 39 /* '<Root>/LED_BLUE' */
AndreaAndreoli 3:06075cbd49bd 40 static uint8_T Controller_Slave_Y_LED_BLUE;
AndreaAndreoli 2:fb694fb2ef9b 41
AndreaAndreoli 3:06075cbd49bd 42 /* '<Root>/SLAVE' */
AndreaAndreoli 3:06075cbd49bd 43 static uint8_T Controller_Slave_Y_SLAVE;
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 3:06075cbd49bd 56 void rt_OneStep(RT_MODEL_Controller_Slave_T *const Controller_Slave_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 3:06075cbd49bd 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 3:06075cbd49bd 66 DigitalOut slave_out(D8);
AndreaAndreoli 3:06075cbd49bd 67 DigitalIn master(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 3:06075cbd49bd 75 Controller_Slave_M->ModelData.blockIO = &Controller_Slave_B;
AndreaAndreoli 3:06075cbd49bd 76 Controller_Slave_M->ModelData.dwork = &Controller_Slave_DW;
AndreaAndreoli 2:fb694fb2ef9b 77
AndreaAndreoli 3:06075cbd49bd 78 Controller_Slave_U_V = V;
AndreaAndreoli 3:06075cbd49bd 79 Controller_Slave_U_D_S = 50;
AndreaAndreoli 3:06075cbd49bd 80 Controller_Slave_U_Master = 1;
AndreaAndreoli 3:06075cbd49bd 81 Controller_Slave_U_QA_EN = true;
AndreaAndreoli 2:fb694fb2ef9b 82
AndreaAndreoli 2:fb694fb2ef9b 83 /* Initialize model */
AndreaAndreoli 3:06075cbd49bd 84 Controller_Slave_initialize(Controller_Slave_M, &Controller_Slave_U_V,
AndreaAndreoli 3:06075cbd49bd 85 &Controller_Slave_U_D_S, &Controller_Slave_U_Master,
AndreaAndreoli 3:06075cbd49bd 86 &Controller_Slave_U_QA_EN, &Controller_Slave_Y_BRAKE,
AndreaAndreoli 3:06075cbd49bd 87 &Controller_Slave_Y_ACC, &Controller_Slave_Y_LED_RED,
AndreaAndreoli 3:06075cbd49bd 88 &Controller_Slave_Y_LED_GREEN, &Controller_Slave_Y_LED_BLUE,
AndreaAndreoli 3:06075cbd49bd 89 &Controller_Slave_Y_SLAVE);
AndreaAndreoli 2:fb694fb2ef9b 90
AndreaAndreoli 3:06075cbd49bd 91 /* Attach rt_OneStep to a timer or interrupt service routine with
AndreaAndreoli 3:06075cbd49bd 92 * period 0.1 seconds (the model's base sample time) here. The
AndreaAndreoli 3:06075cbd49bd 93 * call syntax for rt_OneStep is
AndreaAndreoli 3:06075cbd49bd 94 *
AndreaAndreoli 3:06075cbd49bd 95 * rt_OneStep(Controller_Master_M);
AndreaAndreoli 3:06075cbd49bd 96 */
AndreaAndreoli 2:fb694fb2ef9b 97 t.attach(&step,0.1);
AndreaAndreoli 2:fb694fb2ef9b 98 led_B.write(1);
AndreaAndreoli 2:fb694fb2ef9b 99 led_G.write(1);
AndreaAndreoli 2:fb694fb2ef9b 100 led_R.write(1);
AndreaAndreoli 3:06075cbd49bd 101 master.mode(PullDown);
AndreaAndreoli 3:06075cbd49bd 102 slave_out.write(1);
AndreaAndreoli 2:fb694fb2ef9b 103 while (true) {
AndreaAndreoli 2:fb694fb2ef9b 104 wait(0.2);
AndreaAndreoli 2:fb694fb2ef9b 105 //led_R= !led_R;
AndreaAndreoli 2:fb694fb2ef9b 106
AndreaAndreoli 2:fb694fb2ef9b 107 }
AndreaAndreoli 2:fb694fb2ef9b 108
AndreaAndreoli 2:fb694fb2ef9b 109 /* Disable rt_OneStep() here */
AndreaAndreoli 2:fb694fb2ef9b 110
AndreaAndreoli 2:fb694fb2ef9b 111 /* Terminate model */
AndreaAndreoli 3:06075cbd49bd 112 Controller_Slave_terminate(Controller_Slave_M);
AndreaAndreoli 2:fb694fb2ef9b 113 }
AndreaAndreoli 2:fb694fb2ef9b 114
AndreaAndreoli 2:fb694fb2ef9b 115 void step()
AndreaAndreoli 2:fb694fb2ef9b 116 {
AndreaAndreoli 3:06075cbd49bd 117 Controller_Slave_U_V = V;
AndreaAndreoli 3:06075cbd49bd 118 Controller_Slave_U_D_S = sonic.read_cm();
AndreaAndreoli 3:06075cbd49bd 119 Controller_Slave_U_Master = master.read();
AndreaAndreoli 3:06075cbd49bd 120 Controller_Slave_U_QA_EN = true;
AndreaAndreoli 3:06075cbd49bd 121 rt_OneStep(Controller_Slave_M);
AndreaAndreoli 3:06075cbd49bd 122 slave_out.write(Controller_Slave_Y_SLAVE);
AndreaAndreoli 3:06075cbd49bd 123 pc.printf("blue: %d \n", Controller_Slave_Y_LED_BLUE); // Call read_cm() to get the distance in cm
AndreaAndreoli 3:06075cbd49bd 124 pc.printf("red: %d \n", Controller_Slave_Y_LED_RED);
AndreaAndreoli 3:06075cbd49bd 125 pc.printf("slave out: %d \n", Controller_Slave_Y_SLAVE);
AndreaAndreoli 3:06075cbd49bd 126 pc.printf("master in: %d \n", Controller_Slave_U_Master);
AndreaAndreoli 3:06075cbd49bd 127 pc.printf("brake: %d \n", Controller_Slave_Y_BRAKE);
AndreaAndreoli 3:06075cbd49bd 128 pc.printf("green: %d \n", Controller_Slave_Y_LED_GREEN);
AndreaAndreoli 3:06075cbd49bd 129 pc.printf("acc: %d \n", Controller_Slave_Y_ACC);
AndreaAndreoli 3:06075cbd49bd 130 led_B = !Controller_Slave_Y_LED_BLUE; // negate because 1 -> led off
AndreaAndreoli 3:06075cbd49bd 131 led_R = !Controller_Slave_Y_LED_RED;
AndreaAndreoli 3:06075cbd49bd 132 led_G = !Controller_Slave_Y_LED_GREEN;
AndreaAndreoli 2:fb694fb2ef9b 133 }
AndreaAndreoli 2:fb694fb2ef9b 134
AndreaAndreoli 3:06075cbd49bd 135 void rt_OneStep(RT_MODEL_Controller_Slave_T *const Controller_Slave_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 3:06075cbd49bd 143 rtmSetErrorStatus(Controller_Slave_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 3:06075cbd49bd 154 Controller_Slave_step(Controller_Slave_M, Controller_Slave_U_V,
AndreaAndreoli 3:06075cbd49bd 155 Controller_Slave_U_D_S, Controller_Slave_U_Master,
AndreaAndreoli 3:06075cbd49bd 156 Controller_Slave_U_QA_EN, &Controller_Slave_Y_BRAKE,
AndreaAndreoli 3:06075cbd49bd 157 &Controller_Slave_Y_ACC, &Controller_Slave_Y_LED_RED,
AndreaAndreoli 3:06075cbd49bd 158 &Controller_Slave_Y_LED_GREEN,
AndreaAndreoli 3:06075cbd49bd 159 &Controller_Slave_Y_LED_BLUE, &Controller_Slave_Y_SLAVE);
AndreaAndreoli 2:fb694fb2ef9b 160
AndreaAndreoli 2:fb694fb2ef9b 161 /* Get model outputs here */
AndreaAndreoli 2:fb694fb2ef9b 162
AndreaAndreoli 2:fb694fb2ef9b 163 /* Indicate task complete */
AndreaAndreoli 2:fb694fb2ef9b 164 OverrunFlag = false;
AndreaAndreoli 2:fb694fb2ef9b 165
AndreaAndreoli 2:fb694fb2ef9b 166 /* Disable interrupts here */
AndreaAndreoli 2:fb694fb2ef9b 167 /* Restore FPU context here (if necessary) */
AndreaAndreoli 2:fb694fb2ef9b 168 /* Enable interrupts here */
AndreaAndreoli 2:fb694fb2ef9b 169 }