![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Program that execute AEB system
Dependencies: AEB Ultrasonic Controller_Master mbed
main.cpp@6:d0717be58ca3, 2016-07-10 (annotated)
- Committer:
- AndreaAndreoli
- Date:
- Sun Jul 10 13:18:57 2016 +0000
- Revision:
- 6:d0717be58ca3
- Parent:
- 2:fb694fb2ef9b
- Child:
- 7:6547a25e1867
Added serial read from myrio;
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 | static real_T Controller_Master_U_V; |
AndreaAndreoli | 2:fb694fb2ef9b | 15 | static real_T Controller_Master_U_D_M; |
AndreaAndreoli | 2:fb694fb2ef9b | 16 | static uint8_T Controller_Master_U_Slave; |
AndreaAndreoli | 2:fb694fb2ef9b | 17 | static boolean_T Controller_Master_U_QA_EN; |
AndreaAndreoli | 2:fb694fb2ef9b | 18 | static uint8_T Controller_Master_Y_BRAKE; |
AndreaAndreoli | 2:fb694fb2ef9b | 19 | static uint8_T Controller_Master_Y_ACC; |
AndreaAndreoli | 2:fb694fb2ef9b | 20 | static uint8_T Controller_Master_Y_LED_RED; |
AndreaAndreoli | 2:fb694fb2ef9b | 21 | static uint8_T Controller_Master_Y_LED_GREEN; |
AndreaAndreoli | 2:fb694fb2ef9b | 22 | static uint8_T Controller_Master_Y_LED_BLUE; |
AndreaAndreoli | 2:fb694fb2ef9b | 23 | static uint8_T Controller_Master_Y_MASTER; |
AndreaAndreoli | 2:fb694fb2ef9b | 24 | |
AndreaAndreoli | 2:fb694fb2ef9b | 25 | void rt_OneStep(RT_MODEL_Controller_Master_T *const Controller_Master_M); |
AndreaAndreoli | 2:fb694fb2ef9b | 26 | void step(); |
AndreaAndreoli | 6:d0717be58ca3 | 27 | void cb(); |
AndreaAndreoli | 6:d0717be58ca3 | 28 | void cb() {}; |
AndreaAndreoli | 2:fb694fb2ef9b | 29 | |
AndreaAndreoli | 6:d0717be58ca3 | 30 | Ultrasonic sonic(D6,D7); // Just call this funtion to initialize the ultrasonic sensor |
AndreaAndreoli | 2:fb694fb2ef9b | 31 | // D2 trigger D4 echo |
AndreaAndreoli | 2:fb694fb2ef9b | 32 | DigitalOut led_R(LED_RED); |
AndreaAndreoli | 2:fb694fb2ef9b | 33 | DigitalOut led_G(LED_GREEN); |
AndreaAndreoli | 2:fb694fb2ef9b | 34 | DigitalOut led_B(LED_BLUE); |
AndreaAndreoli | 6:d0717be58ca3 | 35 | DigitalOut master_out(D5); |
AndreaAndreoli | 6:d0717be58ca3 | 36 | DigitalOut acc(D2); |
AndreaAndreoli | 6:d0717be58ca3 | 37 | DigitalOut brake(D3); |
AndreaAndreoli | 6:d0717be58ca3 | 38 | DigitalIn qa(D4); |
AndreaAndreoli | 2:fb694fb2ef9b | 39 | float V = 29; // Set the vehicle speed here |
AndreaAndreoli | 2:fb694fb2ef9b | 40 | Ticker t; |
AndreaAndreoli | 6:d0717be58ca3 | 41 | Timer tim; |
AndreaAndreoli | 2:fb694fb2ef9b | 42 | Serial pc(USBTX, USBRX); // tx, rx |
AndreaAndreoli | 6:d0717be58ca3 | 43 | Serial myrio(D1,D0 ); // tx, rx |
AndreaAndreoli | 2:fb694fb2ef9b | 44 | |
AndreaAndreoli | 2:fb694fb2ef9b | 45 | int main() |
AndreaAndreoli | 2:fb694fb2ef9b | 46 | { |
AndreaAndreoli | 2:fb694fb2ef9b | 47 | /* Pack model data into RTM */ |
AndreaAndreoli | 2:fb694fb2ef9b | 48 | Controller_Master_M->ModelData.blockIO = &Controller_Master_B; |
AndreaAndreoli | 2:fb694fb2ef9b | 49 | Controller_Master_M->ModelData.dwork = &Controller_Master_DW; |
AndreaAndreoli | 2:fb694fb2ef9b | 50 | Controller_Master_U_V = V; |
AndreaAndreoli | 2:fb694fb2ef9b | 51 | Controller_Master_U_D_M = 50; |
AndreaAndreoli | 2:fb694fb2ef9b | 52 | Controller_Master_U_Slave = 1; |
AndreaAndreoli | 2:fb694fb2ef9b | 53 | Controller_Master_U_QA_EN = true; |
AndreaAndreoli | 2:fb694fb2ef9b | 54 | |
AndreaAndreoli | 2:fb694fb2ef9b | 55 | /* Initialize model */ |
AndreaAndreoli | 2:fb694fb2ef9b | 56 | Controller_Master_initialize(Controller_Master_M, &Controller_Master_U_V, |
AndreaAndreoli | 2:fb694fb2ef9b | 57 | &Controller_Master_U_D_M, &Controller_Master_U_Slave, |
AndreaAndreoli | 2:fb694fb2ef9b | 58 | &Controller_Master_U_QA_EN, &Controller_Master_Y_BRAKE, |
AndreaAndreoli | 2:fb694fb2ef9b | 59 | &Controller_Master_Y_ACC, &Controller_Master_Y_LED_RED, |
AndreaAndreoli | 2:fb694fb2ef9b | 60 | &Controller_Master_Y_LED_GREEN, &Controller_Master_Y_LED_BLUE, |
AndreaAndreoli | 2:fb694fb2ef9b | 61 | &Controller_Master_Y_MASTER); |
AndreaAndreoli | 2:fb694fb2ef9b | 62 | |
AndreaAndreoli | 6:d0717be58ca3 | 63 | /* Attach rt_OneStep to a timer or interrupt service routine with |
AndreaAndreoli | 6:d0717be58ca3 | 64 | * period 0.1 seconds (the model's base sample time) here. The |
AndreaAndreoli | 6:d0717be58ca3 | 65 | * call syntax for rt_OneStep is |
AndreaAndreoli | 6:d0717be58ca3 | 66 | * |
AndreaAndreoli | 6:d0717be58ca3 | 67 | * rt_OneStep(Controller_Master_M); |
AndreaAndreoli | 6:d0717be58ca3 | 68 | */ |
AndreaAndreoli | 6:d0717be58ca3 | 69 | |
AndreaAndreoli | 2:fb694fb2ef9b | 70 | led_B.write(1); |
AndreaAndreoli | 2:fb694fb2ef9b | 71 | led_G.write(1); |
AndreaAndreoli | 2:fb694fb2ef9b | 72 | led_R.write(1); |
AndreaAndreoli | 6:d0717be58ca3 | 73 | qa.mode(PullDown); |
AndreaAndreoli | 6:d0717be58ca3 | 74 | master_out = 1; |
AndreaAndreoli | 6:d0717be58ca3 | 75 | acc = 0; |
AndreaAndreoli | 6:d0717be58ca3 | 76 | brake = 0; |
AndreaAndreoli | 6:d0717be58ca3 | 77 | t.attach(&step,0.1); |
AndreaAndreoli | 6:d0717be58ca3 | 78 | |
AndreaAndreoli | 2:fb694fb2ef9b | 79 | while (true) { |
AndreaAndreoli | 2:fb694fb2ef9b | 80 | wait(0.2); |
AndreaAndreoli | 2:fb694fb2ef9b | 81 | } |
AndreaAndreoli | 2:fb694fb2ef9b | 82 | |
AndreaAndreoli | 2:fb694fb2ef9b | 83 | /* Disable rt_OneStep() here */ |
AndreaAndreoli | 2:fb694fb2ef9b | 84 | |
AndreaAndreoli | 2:fb694fb2ef9b | 85 | /* Terminate model */ |
AndreaAndreoli | 2:fb694fb2ef9b | 86 | Controller_Master_terminate(Controller_Master_M); |
AndreaAndreoli | 2:fb694fb2ef9b | 87 | } |
AndreaAndreoli | 2:fb694fb2ef9b | 88 | |
AndreaAndreoli | 2:fb694fb2ef9b | 89 | void step() |
AndreaAndreoli | 2:fb694fb2ef9b | 90 | { |
AndreaAndreoli | 6:d0717be58ca3 | 91 | int var = 0; |
AndreaAndreoli | 6:d0717be58ca3 | 92 | char str[10]; |
AndreaAndreoli | 6:d0717be58ca3 | 93 | if(myrio.readable()) { |
AndreaAndreoli | 6:d0717be58ca3 | 94 | myrio.scanf("%s",str); |
AndreaAndreoli | 6:d0717be58ca3 | 95 | var = atoi(str); |
AndreaAndreoli | 6:d0717be58ca3 | 96 | pc.printf("%d \n",var); |
AndreaAndreoli | 6:d0717be58ca3 | 97 | } |
AndreaAndreoli | 6:d0717be58ca3 | 98 | V = var; |
AndreaAndreoli | 2:fb694fb2ef9b | 99 | Controller_Master_U_V = V; |
AndreaAndreoli | 2:fb694fb2ef9b | 100 | Controller_Master_U_D_M = sonic.read_cm(); |
AndreaAndreoli | 6:d0717be58ca3 | 101 | Controller_Master_U_Slave = 1; |
AndreaAndreoli | 6:d0717be58ca3 | 102 | Controller_Master_U_QA_EN = qa; |
AndreaAndreoli | 2:fb694fb2ef9b | 103 | rt_OneStep(Controller_Master_M); |
AndreaAndreoli | 6:d0717be58ca3 | 104 | master_out = Controller_Master_Y_MASTER; |
AndreaAndreoli | 6:d0717be58ca3 | 105 | acc = Controller_Master_Y_ACC; |
AndreaAndreoli | 6:d0717be58ca3 | 106 | brake = Controller_Master_Y_BRAKE; |
AndreaAndreoli | 2:fb694fb2ef9b | 107 | led_B = !Controller_Master_Y_LED_BLUE; // negate because 1 -> led off |
AndreaAndreoli | 2:fb694fb2ef9b | 108 | led_R = !Controller_Master_Y_LED_RED; |
AndreaAndreoli | 2:fb694fb2ef9b | 109 | led_G = !Controller_Master_Y_LED_GREEN; |
AndreaAndreoli | 6:d0717be58ca3 | 110 | pc.printf("distance: %f \n", Controller_Master_U_D_M); |
AndreaAndreoli | 6:d0717be58ca3 | 111 | //pc.printf("speed: %f \n", Controller_Master_U_V); |
AndreaAndreoli | 6:d0717be58ca3 | 112 | /* pc.printf("blue: %d \n", Controller_Master_Y_LED_BLUE); // Call read_cm() to get the distance in cm |
AndreaAndreoli | 6:d0717be58ca3 | 113 | pc.printf("red: %d \n", Controller_Master_Y_LED_RED); |
AndreaAndreoli | 6:d0717be58ca3 | 114 | pc.printf("master: %d \n", Controller_Master_Y_MASTER); |
AndreaAndreoli | 6:d0717be58ca3 | 115 | pc.printf("brake: %d \n", Controller_Master_Y_BRAKE); |
AndreaAndreoli | 6:d0717be58ca3 | 116 | pc.printf("green: %d \n", Controller_Master_Y_LED_GREEN); |
AndreaAndreoli | 6:d0717be58ca3 | 117 | pc.printf("acc: %d \n", Controller_Master_Y_ACC); |
AndreaAndreoli | 6:d0717be58ca3 | 118 | */ |
AndreaAndreoli | 6:d0717be58ca3 | 119 | |
AndreaAndreoli | 2:fb694fb2ef9b | 120 | } |
AndreaAndreoli | 2:fb694fb2ef9b | 121 | |
AndreaAndreoli | 2:fb694fb2ef9b | 122 | void rt_OneStep(RT_MODEL_Controller_Master_T *const Controller_Master_M) |
AndreaAndreoli | 2:fb694fb2ef9b | 123 | { |
AndreaAndreoli | 2:fb694fb2ef9b | 124 | static boolean_T OverrunFlag = false; |
AndreaAndreoli | 2:fb694fb2ef9b | 125 | |
AndreaAndreoli | 2:fb694fb2ef9b | 126 | /* Disable interrupts here */ |
AndreaAndreoli | 2:fb694fb2ef9b | 127 | |
AndreaAndreoli | 2:fb694fb2ef9b | 128 | /* Check for overrun */ |
AndreaAndreoli | 2:fb694fb2ef9b | 129 | if (OverrunFlag) { |
AndreaAndreoli | 2:fb694fb2ef9b | 130 | rtmSetErrorStatus(Controller_Master_M, "Overrun"); |
AndreaAndreoli | 2:fb694fb2ef9b | 131 | return; |
AndreaAndreoli | 2:fb694fb2ef9b | 132 | } |
AndreaAndreoli | 2:fb694fb2ef9b | 133 | |
AndreaAndreoli | 2:fb694fb2ef9b | 134 | OverrunFlag = true; |
AndreaAndreoli | 2:fb694fb2ef9b | 135 | |
AndreaAndreoli | 2:fb694fb2ef9b | 136 | /* Save FPU context here (if necessary) */ |
AndreaAndreoli | 2:fb694fb2ef9b | 137 | /* Re-enable timer or interrupt here */ |
AndreaAndreoli | 2:fb694fb2ef9b | 138 | /* Set model inputs here */ |
AndreaAndreoli | 2:fb694fb2ef9b | 139 | |
AndreaAndreoli | 2:fb694fb2ef9b | 140 | /* Step the model */ |
AndreaAndreoli | 2:fb694fb2ef9b | 141 | Controller_Master_step(Controller_Master_M, Controller_Master_U_V, |
AndreaAndreoli | 2:fb694fb2ef9b | 142 | Controller_Master_U_D_M, Controller_Master_U_QA_EN, |
AndreaAndreoli | 2:fb694fb2ef9b | 143 | &Controller_Master_Y_BRAKE, &Controller_Master_Y_ACC, |
AndreaAndreoli | 2:fb694fb2ef9b | 144 | &Controller_Master_Y_LED_RED, &Controller_Master_Y_LED_GREEN, |
AndreaAndreoli | 2:fb694fb2ef9b | 145 | &Controller_Master_Y_LED_BLUE, &Controller_Master_Y_MASTER); |
AndreaAndreoli | 2:fb694fb2ef9b | 146 | |
AndreaAndreoli | 2:fb694fb2ef9b | 147 | /* Get model outputs here */ |
AndreaAndreoli | 2:fb694fb2ef9b | 148 | |
AndreaAndreoli | 2:fb694fb2ef9b | 149 | /* Indicate task complete */ |
AndreaAndreoli | 2:fb694fb2ef9b | 150 | OverrunFlag = false; |
AndreaAndreoli | 2:fb694fb2ef9b | 151 | |
AndreaAndreoli | 2:fb694fb2ef9b | 152 | /* Disable interrupts here */ |
AndreaAndreoli | 2:fb694fb2ef9b | 153 | /* Restore FPU context here (if necessary) */ |
AndreaAndreoli | 2:fb694fb2ef9b | 154 | /* Enable interrupts here */ |
AndreaAndreoli | 2:fb694fb2ef9b | 155 | } |