Program that execute AEB system
Dependencies: AEB Ultrasonic Controller_Master mbed
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?
User | Revision | Line number | New 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 | } |