MBSD_assignment / Mbed 2 deprecated AEB

Program that execute AEB system

Dependencies:   AEB Ultrasonic Controller_Master mbed

Dependents:   AEB

main.cpp@4:0df8a61cbee9, 2016-07-11 (annotated)

Committer:
AndreaAndreoli
Date:
Mon Jul 11 10:16:14 2016 +0000
Revision:
4:0df8a61cbee9
Parent:
3:06075cbd49bd 
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 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 3:06075cbd49bd 14 static real_T Controller_Slave_U_V;
AndreaAndreoli 3:06075cbd49bd 15 static real_T Controller_Slave_U_D_S;
AndreaAndreoli 3:06075cbd49bd 16 static uint8_T Controller_Slave_U_Master;
AndreaAndreoli 3:06075cbd49bd 17 static boolean_T Controller_Slave_U_QA_EN;
AndreaAndreoli 3:06075cbd49bd 18 static uint8_T Controller_Slave_Y_BRAKE;
AndreaAndreoli 3:06075cbd49bd 19 static uint8_T Controller_Slave_Y_ACC;
AndreaAndreoli 3:06075cbd49bd 20 static uint8_T Controller_Slave_Y_LED_RED;
AndreaAndreoli 3:06075cbd49bd 21 static uint8_T Controller_Slave_Y_LED_GREEN;
AndreaAndreoli 3:06075cbd49bd 22 static uint8_T Controller_Slave_Y_LED_BLUE;
AndreaAndreoli 3:06075cbd49bd 23 static uint8_T Controller_Slave_Y_SLAVE;
AndreaAndreoli 3:06075cbd49bd 24 void rt_OneStep(RT_MODEL_Controller_Slave_T *const Controller_Slave_M);
AndreaAndreoli 2:fb694fb2ef9b 25 void step();
AndreaAndreoli 2:fb694fb2ef9b 26
AndreaAndreoli 2:fb694fb2ef9b 27
AndreaAndreoli 4:0df8a61cbee9 28 Ultrasonic sonic(D6,D7); // Just call this funtion to initialize the ultrasonic sensor
AndreaAndreoli 2:fb694fb2ef9b 29 // D2 trigger D4 echo
AndreaAndreoli 2:fb694fb2ef9b 30 DigitalOut led_R(LED_RED);
AndreaAndreoli 2:fb694fb2ef9b 31 DigitalOut led_G(LED_GREEN);
AndreaAndreoli 2:fb694fb2ef9b 32 DigitalOut led_B(LED_BLUE);
AndreaAndreoli 4:0df8a61cbee9 33 DigitalIn master(D5);
AndreaAndreoli 4:0df8a61cbee9 34 DigitalOut acc(D2);
AndreaAndreoli 4:0df8a61cbee9 35 DigitalOut brake(D3);
AndreaAndreoli 4:0df8a61cbee9 36 DigitalIn qa(D4);
AndreaAndreoli 2:fb694fb2ef9b 37 float V = 29; // Set the vehicle speed here
AndreaAndreoli 2:fb694fb2ef9b 38 Ticker t;
AndreaAndreoli 2:fb694fb2ef9b 39 Serial pc(USBTX, USBRX); // tx, rx
AndreaAndreoli 4:0df8a61cbee9 40 Serial myrio(D1,D0 ); // tx, rx
AndreaAndreoli 2:fb694fb2ef9b 41
AndreaAndreoli 2:fb694fb2ef9b 42 int main()
AndreaAndreoli 2:fb694fb2ef9b 43 {
AndreaAndreoli 2:fb694fb2ef9b 44 /* Pack model data into RTM */
AndreaAndreoli 3:06075cbd49bd 45 Controller_Slave_M->ModelData.blockIO = &Controller_Slave_B;
AndreaAndreoli 3:06075cbd49bd 46 Controller_Slave_M->ModelData.dwork = &Controller_Slave_DW;
AndreaAndreoli 2:fb694fb2ef9b 47
AndreaAndreoli 3:06075cbd49bd 48 Controller_Slave_U_V = V;
AndreaAndreoli 3:06075cbd49bd 49 Controller_Slave_U_D_S = 50;
AndreaAndreoli 3:06075cbd49bd 50 Controller_Slave_U_Master = 1;
AndreaAndreoli 3:06075cbd49bd 51 Controller_Slave_U_QA_EN = true;
AndreaAndreoli 2:fb694fb2ef9b 52
AndreaAndreoli 2:fb694fb2ef9b 53 /* Initialize model */
AndreaAndreoli 3:06075cbd49bd 54 Controller_Slave_initialize(Controller_Slave_M, &Controller_Slave_U_V,
AndreaAndreoli 3:06075cbd49bd 55 &Controller_Slave_U_D_S, &Controller_Slave_U_Master,
AndreaAndreoli 3:06075cbd49bd 56 &Controller_Slave_U_QA_EN, &Controller_Slave_Y_BRAKE,
AndreaAndreoli 3:06075cbd49bd 57 &Controller_Slave_Y_ACC, &Controller_Slave_Y_LED_RED,
AndreaAndreoli 3:06075cbd49bd 58 &Controller_Slave_Y_LED_GREEN, &Controller_Slave_Y_LED_BLUE,
AndreaAndreoli 3:06075cbd49bd 59 &Controller_Slave_Y_SLAVE);
AndreaAndreoli 2:fb694fb2ef9b 60
AndreaAndreoli 4:0df8a61cbee9 61
AndreaAndreoli 2:fb694fb2ef9b 62 t.attach(&step,0.1);
AndreaAndreoli 2:fb694fb2ef9b 63 led_B.write(1);
AndreaAndreoli 2:fb694fb2ef9b 64 led_G.write(1);
AndreaAndreoli 2:fb694fb2ef9b 65 led_R.write(1);
AndreaAndreoli 3:06075cbd49bd 66 master.mode(PullDown);
AndreaAndreoli 4:0df8a61cbee9 67 // slave_out.write(1);
AndreaAndreoli 2:fb694fb2ef9b 68 while (true) {
AndreaAndreoli 2:fb694fb2ef9b 69 wait(0.2);
AndreaAndreoli 2:fb694fb2ef9b 70 }
AndreaAndreoli 3:06075cbd49bd 71 Controller_Slave_terminate(Controller_Slave_M);
AndreaAndreoli 2:fb694fb2ef9b 72 }
AndreaAndreoli 2:fb694fb2ef9b 73
AndreaAndreoli 2:fb694fb2ef9b 74 void step()
AndreaAndreoli 2:fb694fb2ef9b 75 {
AndreaAndreoli 4:0df8a61cbee9 76 int var = 0;
AndreaAndreoli 4:0df8a61cbee9 77 char str[10];
AndreaAndreoli 4:0df8a61cbee9 78 if(myrio.readable()) {
AndreaAndreoli 4:0df8a61cbee9 79 myrio.scanf("%s",str);
AndreaAndreoli 4:0df8a61cbee9 80 var = atoi(str);
AndreaAndreoli 4:0df8a61cbee9 81 pc.printf("%d \n",var);
AndreaAndreoli 4:0df8a61cbee9 82 }
AndreaAndreoli 4:0df8a61cbee9 83 V = var;
AndreaAndreoli 3:06075cbd49bd 84 Controller_Slave_U_V = V;
AndreaAndreoli 3:06075cbd49bd 85 Controller_Slave_U_D_S = sonic.read_cm();
AndreaAndreoli 3:06075cbd49bd 86 Controller_Slave_U_Master = master.read();
AndreaAndreoli 4:0df8a61cbee9 87 Controller_Slave_U_QA_EN = qa;
AndreaAndreoli 3:06075cbd49bd 88 rt_OneStep(Controller_Slave_M);
AndreaAndreoli 4:0df8a61cbee9 89 // slave_out.write(Controller_Slave_Y_SLAVE);
AndreaAndreoli 4:0df8a61cbee9 90 acc = Controller_Slave_Y_ACC;
AndreaAndreoli 4:0df8a61cbee9 91 brake = Controller_Slave_Y_BRAKE;
AndreaAndreoli 3:06075cbd49bd 92 led_B = !Controller_Slave_Y_LED_BLUE; // negate because 1 -> led off
AndreaAndreoli 3:06075cbd49bd 93 led_R = !Controller_Slave_Y_LED_RED;
AndreaAndreoli 3:06075cbd49bd 94 led_G = !Controller_Slave_Y_LED_GREEN;
AndreaAndreoli 4:0df8a61cbee9 95 // pc.printf("blue: %d \n", Controller_Slave_Y_LED_BLUE); // Call read_cm() to get the distance in cm
AndreaAndreoli 4:0df8a61cbee9 96 // pc.printf("red: %d \n", Controller_Slave_Y_LED_RED);
AndreaAndreoli 4:0df8a61cbee9 97 // pc.printf("slave out: %d \n", Controller_Slave_Y_SLAVE);
AndreaAndreoli 4:0df8a61cbee9 98 // pc.printf("master in: %d \n", Controller_Slave_U_Master);
AndreaAndreoli 4:0df8a61cbee9 99 // pc.printf("brake: %d \n", Controller_Slave_Y_BRAKE);
AndreaAndreoli 4:0df8a61cbee9 100 // pc.printf("green: %d \n", Controller_Slave_Y_LED_GREEN);
AndreaAndreoli 4:0df8a61cbee9 101 // pc.printf("acc: %d \n", Controller_Slave_Y_ACC);
AndreaAndreoli 4:0df8a61cbee9 102
AndreaAndreoli 2:fb694fb2ef9b 103 }
AndreaAndreoli 2:fb694fb2ef9b 104
AndreaAndreoli 3:06075cbd49bd 105 void rt_OneStep(RT_MODEL_Controller_Slave_T *const Controller_Slave_M)
AndreaAndreoli 2:fb694fb2ef9b 106 {
AndreaAndreoli 2:fb694fb2ef9b 107 static boolean_T OverrunFlag = false;
AndreaAndreoli 2:fb694fb2ef9b 108
AndreaAndreoli 2:fb694fb2ef9b 109 /* Disable interrupts here */
AndreaAndreoli 2:fb694fb2ef9b 110
AndreaAndreoli 2:fb694fb2ef9b 111 /* Check for overrun */
AndreaAndreoli 2:fb694fb2ef9b 112 if (OverrunFlag) {
AndreaAndreoli 3:06075cbd49bd 113 rtmSetErrorStatus(Controller_Slave_M, "Overrun");
AndreaAndreoli 2:fb694fb2ef9b 114 return;
AndreaAndreoli 2:fb694fb2ef9b 115 }
AndreaAndreoli 2:fb694fb2ef9b 116
AndreaAndreoli 2:fb694fb2ef9b 117 OverrunFlag = true;
AndreaAndreoli 2:fb694fb2ef9b 118
AndreaAndreoli 2:fb694fb2ef9b 119 /* Save FPU context here (if necessary) */
AndreaAndreoli 2:fb694fb2ef9b 120 /* Re-enable timer or interrupt here */
AndreaAndreoli 2:fb694fb2ef9b 121 /* Set model inputs here */
AndreaAndreoli 2:fb694fb2ef9b 122
AndreaAndreoli 2:fb694fb2ef9b 123 /* Step the model */
AndreaAndreoli 3:06075cbd49bd 124 Controller_Slave_step(Controller_Slave_M, Controller_Slave_U_V,
AndreaAndreoli 3:06075cbd49bd 125 Controller_Slave_U_D_S, Controller_Slave_U_Master,
AndreaAndreoli 3:06075cbd49bd 126 Controller_Slave_U_QA_EN, &Controller_Slave_Y_BRAKE,
AndreaAndreoli 3:06075cbd49bd 127 &Controller_Slave_Y_ACC, &Controller_Slave_Y_LED_RED,
AndreaAndreoli 3:06075cbd49bd 128 &Controller_Slave_Y_LED_GREEN,
AndreaAndreoli 3:06075cbd49bd 129 &Controller_Slave_Y_LED_BLUE, &Controller_Slave_Y_SLAVE);
AndreaAndreoli 2:fb694fb2ef9b 130
AndreaAndreoli 2:fb694fb2ef9b 131 /* Get model outputs here */
AndreaAndreoli 2:fb694fb2ef9b 132
AndreaAndreoli 2:fb694fb2ef9b 133 /* Indicate task complete */
AndreaAndreoli 2:fb694fb2ef9b 134 OverrunFlag = false;
AndreaAndreoli 2:fb694fb2ef9b 135
AndreaAndreoli 2:fb694fb2ef9b 136 /* Disable interrupts here */
AndreaAndreoli 2:fb694fb2ef9b 137 /* Restore FPU context here (if necessary) */
AndreaAndreoli 2:fb694fb2ef9b 138 /* Enable interrupts here */
AndreaAndreoli 2:fb694fb2ef9b 139 }