MBSD_assignment / Mbed 2 deprecated AEB

Program that execute AEB system

Dependencies:   AEB Ultrasonic Controller_Master mbed

Dependents:   AEB

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?

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 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 }