MBSD_assignment / Mbed 2 deprecated AEB

Program that execute AEB system

Dependencies:   AEB Ultrasonic Controller_Master mbed

Dependents:   AEB

main.cpp@7:6547a25e1867, 2016-07-11 (annotated)

Committer:
AndreaAndreoli
Date:
Mon Jul 11 10:17:50 2016 +0000
Revision:
7:6547a25e1867
Parent:
6:d0717be58ca3 
bug fixed;

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 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 2:fb694fb2ef9b 41 Serial pc(USBTX, USBRX); // tx, rx
AndreaAndreoli 6:d0717be58ca3 42 Serial myrio(D1,D0 ); // tx, rx
AndreaAndreoli 2:fb694fb2ef9b 43
AndreaAndreoli 2:fb694fb2ef9b 44 int main()
AndreaAndreoli 2:fb694fb2ef9b 45 {
AndreaAndreoli 2:fb694fb2ef9b 46 /* Pack model data into RTM */
AndreaAndreoli 2:fb694fb2ef9b 47 Controller_Master_M->ModelData.blockIO = &Controller_Master_B;
AndreaAndreoli 2:fb694fb2ef9b 48 Controller_Master_M->ModelData.dwork = &Controller_Master_DW;
AndreaAndreoli 2:fb694fb2ef9b 49 Controller_Master_U_V = V;
AndreaAndreoli 2:fb694fb2ef9b 50 Controller_Master_U_D_M = 50;
AndreaAndreoli 2:fb694fb2ef9b 51 Controller_Master_U_Slave = 1;
AndreaAndreoli 2:fb694fb2ef9b 52 Controller_Master_U_QA_EN = true;
AndreaAndreoli 2:fb694fb2ef9b 53
AndreaAndreoli 2:fb694fb2ef9b 54 /* Initialize model */
AndreaAndreoli 2:fb694fb2ef9b 55 Controller_Master_initialize(Controller_Master_M, &Controller_Master_U_V,
AndreaAndreoli 2:fb694fb2ef9b 56 &Controller_Master_U_D_M, &Controller_Master_U_Slave,
AndreaAndreoli 2:fb694fb2ef9b 57 &Controller_Master_U_QA_EN, &Controller_Master_Y_BRAKE,
AndreaAndreoli 2:fb694fb2ef9b 58 &Controller_Master_Y_ACC, &Controller_Master_Y_LED_RED,
AndreaAndreoli 2:fb694fb2ef9b 59 &Controller_Master_Y_LED_GREEN, &Controller_Master_Y_LED_BLUE,
AndreaAndreoli 2:fb694fb2ef9b 60 &Controller_Master_Y_MASTER);
AndreaAndreoli 2:fb694fb2ef9b 61
AndreaAndreoli 6:d0717be58ca3 62 /* Attach rt_OneStep to a timer or interrupt service routine with
AndreaAndreoli 6:d0717be58ca3 63 * period 0.1 seconds (the model's base sample time) here. The
AndreaAndreoli 6:d0717be58ca3 64 * call syntax for rt_OneStep is
AndreaAndreoli 6:d0717be58ca3 65 *
AndreaAndreoli 6:d0717be58ca3 66 * rt_OneStep(Controller_Master_M);
AndreaAndreoli 6:d0717be58ca3 67 */
AndreaAndreoli 6:d0717be58ca3 68
AndreaAndreoli 2:fb694fb2ef9b 69 led_B.write(1);
AndreaAndreoli 2:fb694fb2ef9b 70 led_G.write(1);
AndreaAndreoli 2:fb694fb2ef9b 71 led_R.write(1);
AndreaAndreoli 6:d0717be58ca3 72 qa.mode(PullDown);
AndreaAndreoli 6:d0717be58ca3 73 master_out = 1;
AndreaAndreoli 6:d0717be58ca3 74 acc = 0;
AndreaAndreoli 6:d0717be58ca3 75 brake = 0;
AndreaAndreoli 6:d0717be58ca3 76 t.attach(&step,0.1);
AndreaAndreoli 6:d0717be58ca3 77
AndreaAndreoli 2:fb694fb2ef9b 78 while (true) {
AndreaAndreoli 2:fb694fb2ef9b 79 wait(0.2);
AndreaAndreoli 2:fb694fb2ef9b 80 }
AndreaAndreoli 2:fb694fb2ef9b 81
AndreaAndreoli 2:fb694fb2ef9b 82 /* Disable rt_OneStep() here */
AndreaAndreoli 2:fb694fb2ef9b 83
AndreaAndreoli 2:fb694fb2ef9b 84 /* Terminate model */
AndreaAndreoli 2:fb694fb2ef9b 85 Controller_Master_terminate(Controller_Master_M);
AndreaAndreoli 2:fb694fb2ef9b 86 }
AndreaAndreoli 2:fb694fb2ef9b 87
AndreaAndreoli 2:fb694fb2ef9b 88 void step()
AndreaAndreoli 2:fb694fb2ef9b 89 {
AndreaAndreoli 6:d0717be58ca3 90 int var = 0;
AndreaAndreoli 6:d0717be58ca3 91 char str[10];
AndreaAndreoli 6:d0717be58ca3 92 if(myrio.readable()) {
AndreaAndreoli 6:d0717be58ca3 93 myrio.scanf("%s",str);
AndreaAndreoli 6:d0717be58ca3 94 var = atoi(str);
AndreaAndreoli 7:6547a25e1867 95 //pc.printf("%d \n",var);
AndreaAndreoli 6:d0717be58ca3 96 }
AndreaAndreoli 6:d0717be58ca3 97 V = var;
AndreaAndreoli 2:fb694fb2ef9b 98 Controller_Master_U_V = V;
AndreaAndreoli 2:fb694fb2ef9b 99 Controller_Master_U_D_M = sonic.read_cm();
AndreaAndreoli 6:d0717be58ca3 100 Controller_Master_U_Slave = 1;
AndreaAndreoli 6:d0717be58ca3 101 Controller_Master_U_QA_EN = qa;
AndreaAndreoli 2:fb694fb2ef9b 102 rt_OneStep(Controller_Master_M);
AndreaAndreoli 6:d0717be58ca3 103 master_out = Controller_Master_Y_MASTER;
AndreaAndreoli 6:d0717be58ca3 104 acc = Controller_Master_Y_ACC;
AndreaAndreoli 6:d0717be58ca3 105 brake = Controller_Master_Y_BRAKE;
AndreaAndreoli 2:fb694fb2ef9b 106 led_B = !Controller_Master_Y_LED_BLUE; // negate because 1 -> led off
AndreaAndreoli 2:fb694fb2ef9b 107 led_R = !Controller_Master_Y_LED_RED;
AndreaAndreoli 2:fb694fb2ef9b 108 led_G = !Controller_Master_Y_LED_GREEN;
AndreaAndreoli 6:d0717be58ca3 109 pc.printf("distance: %f \n", Controller_Master_U_D_M);
AndreaAndreoli 7:6547a25e1867 110 pc.printf("speed: %f \n", Controller_Master_U_V);
AndreaAndreoli 6:d0717be58ca3 111 /* pc.printf("blue: %d \n", Controller_Master_Y_LED_BLUE); // Call read_cm() to get the distance in cm
AndreaAndreoli 6:d0717be58ca3 112 pc.printf("red: %d \n", Controller_Master_Y_LED_RED);
AndreaAndreoli 6:d0717be58ca3 113 pc.printf("master: %d \n", Controller_Master_Y_MASTER);
AndreaAndreoli 6:d0717be58ca3 114 pc.printf("brake: %d \n", Controller_Master_Y_BRAKE);
AndreaAndreoli 6:d0717be58ca3 115 pc.printf("green: %d \n", Controller_Master_Y_LED_GREEN);
AndreaAndreoli 6:d0717be58ca3 116 pc.printf("acc: %d \n", Controller_Master_Y_ACC);
AndreaAndreoli 6:d0717be58ca3 117 */
AndreaAndreoli 6:d0717be58ca3 118
AndreaAndreoli 2:fb694fb2ef9b 119 }
AndreaAndreoli 2:fb694fb2ef9b 120
AndreaAndreoli 2:fb694fb2ef9b 121 void rt_OneStep(RT_MODEL_Controller_Master_T *const Controller_Master_M)
AndreaAndreoli 2:fb694fb2ef9b 122 {
AndreaAndreoli 2:fb694fb2ef9b 123 static boolean_T OverrunFlag = false;
AndreaAndreoli 2:fb694fb2ef9b 124
AndreaAndreoli 2:fb694fb2ef9b 125 /* Disable interrupts here */
AndreaAndreoli 2:fb694fb2ef9b 126
AndreaAndreoli 2:fb694fb2ef9b 127 /* Check for overrun */
AndreaAndreoli 2:fb694fb2ef9b 128 if (OverrunFlag) {
AndreaAndreoli 2:fb694fb2ef9b 129 rtmSetErrorStatus(Controller_Master_M, "Overrun");
AndreaAndreoli 2:fb694fb2ef9b 130 return;
AndreaAndreoli 2:fb694fb2ef9b 131 }
AndreaAndreoli 2:fb694fb2ef9b 132
AndreaAndreoli 2:fb694fb2ef9b 133 OverrunFlag = true;
AndreaAndreoli 2:fb694fb2ef9b 134
AndreaAndreoli 2:fb694fb2ef9b 135 /* Save FPU context here (if necessary) */
AndreaAndreoli 2:fb694fb2ef9b 136 /* Re-enable timer or interrupt here */
AndreaAndreoli 2:fb694fb2ef9b 137 /* Set model inputs here */
AndreaAndreoli 2:fb694fb2ef9b 138
AndreaAndreoli 2:fb694fb2ef9b 139 /* Step the model */
AndreaAndreoli 2:fb694fb2ef9b 140 Controller_Master_step(Controller_Master_M, Controller_Master_U_V,
AndreaAndreoli 2:fb694fb2ef9b 141 Controller_Master_U_D_M, Controller_Master_U_QA_EN,
AndreaAndreoli 2:fb694fb2ef9b 142 &Controller_Master_Y_BRAKE, &Controller_Master_Y_ACC,
AndreaAndreoli 2:fb694fb2ef9b 143 &Controller_Master_Y_LED_RED, &Controller_Master_Y_LED_GREEN,
AndreaAndreoli 2:fb694fb2ef9b 144 &Controller_Master_Y_LED_BLUE, &Controller_Master_Y_MASTER);
AndreaAndreoli 2:fb694fb2ef9b 145
AndreaAndreoli 2:fb694fb2ef9b 146 /* Get model outputs here */
AndreaAndreoli 2:fb694fb2ef9b 147
AndreaAndreoli 2:fb694fb2ef9b 148 /* Indicate task complete */
AndreaAndreoli 2:fb694fb2ef9b 149 OverrunFlag = false;
AndreaAndreoli 2:fb694fb2ef9b 150
AndreaAndreoli 2:fb694fb2ef9b 151 /* Disable interrupts here */
AndreaAndreoli 2:fb694fb2ef9b 152 /* Restore FPU context here (if necessary) */
AndreaAndreoli 2:fb694fb2ef9b 153 /* Enable interrupts here */
AndreaAndreoli 2:fb694fb2ef9b 154 }