MBSD / Mbed 2 deprecated AEB_TERATERM

d

Dependencies:   mbed

Fork of AEB by Vincenzo Comito

main.cpp@0:9d530d56a118, 2016-07-24 (annotated)

Committer:
clynamen
Date:
Sun Jul 24 14:42:26 2016 +0000
Revision:
0:9d530d56a118
Child:
1:45911e86ffee
violante;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
clynamen 0:9d530d56a118 1 #include "mbed.h"
clynamen 0:9d530d56a118 2
clynamen 0:9d530d56a118 3 #include "controller.h"
clynamen 0:9d530d56a118 4 #include "rtwtypes.h"
clynamen 0:9d530d56a118 5
clynamen 0:9d530d56a118 6 DigitalOut led(LED_RED);
clynamen 0:9d530d56a118 7 DigitalOut trigger(D2);
clynamen 0:9d530d56a118 8 InterruptIn echo(D4);
clynamen 0:9d530d56a118 9 Timer t;
clynamen 0:9d530d56a118 10 Ticker scheduler;
clynamen 0:9d530d56a118 11
clynamen 0:9d530d56a118 12 float distance;
clynamen 0:9d530d56a118 13
clynamen 0:9d530d56a118 14 Serial pc(USBTX, USBRX); // tx, rx
clynamen 0:9d530d56a118 15
clynamen 0:9d530d56a118 16
clynamen 0:9d530d56a118 17 //
clynamen 0:9d530d56a118 18 // Copy from ert_main.c
clynamen 0:9d530d56a118 19 //
clynamen 0:9d530d56a118 20 static RT_MODEL_Controller_T Controller_M_;
clynamen 0:9d530d56a118 21 static RT_MODEL_Controller_T *const Controller_M = &Controller_M_;/* Real-time model */
clynamen 0:9d530d56a118 22 static DW_Controller_T Controller_DW; /* Observable states */
clynamen 0:9d530d56a118 23
clynamen 0:9d530d56a118 24 /* '<Root>/distance' */
clynamen 0:9d530d56a118 25 static real32_T Controller_U_distance;
clynamen 0:9d530d56a118 26
clynamen 0:9d530d56a118 27 /* '<Root>/led' */
clynamen 0:9d530d56a118 28 static uint8_T Controller_Y_led;
clynamen 0:9d530d56a118 29
clynamen 0:9d530d56a118 30 /*
clynamen 0:9d530d56a118 31 * Associating rt_OneStep with a real-time clock or interrupt service routine
clynamen 0:9d530d56a118 32 * is what makes the generated code "real-time". The function rt_OneStep is
clynamen 0:9d530d56a118 33 * always associated with the base rate of the model. Subrates are managed
clynamen 0:9d530d56a118 34 * by the base rate from inside the generated code. Enabling/disabling
clynamen 0:9d530d56a118 35 * interrupts and floating point context switches are target specific. This
clynamen 0:9d530d56a118 36 * example code indicates where these should take place relative to executing
clynamen 0:9d530d56a118 37 * the generated code step function. Overrun behavior should be tailored to
clynamen 0:9d530d56a118 38 * your application needs. This example simply sets an error status in the
clynamen 0:9d530d56a118 39 * real-time model and returns from rt_OneStep.
clynamen 0:9d530d56a118 40 */
clynamen 0:9d530d56a118 41 void rt_OneStep(RT_MODEL_Controller_T *const Controller_M);
clynamen 0:9d530d56a118 42 void rt_OneStep(RT_MODEL_Controller_T *const Controller_M)
clynamen 0:9d530d56a118 43 {
clynamen 0:9d530d56a118 44 static boolean_T OverrunFlag = false;
clynamen 0:9d530d56a118 45
clynamen 0:9d530d56a118 46 /* Disable interrupts here */
clynamen 0:9d530d56a118 47
clynamen 0:9d530d56a118 48 /* Check for overrun */
clynamen 0:9d530d56a118 49 if (OverrunFlag) {
clynamen 0:9d530d56a118 50 rtmSetErrorStatus(Controller_M, "Overrun");
clynamen 0:9d530d56a118 51 return;
clynamen 0:9d530d56a118 52 }
clynamen 0:9d530d56a118 53
clynamen 0:9d530d56a118 54 OverrunFlag = true;
clynamen 0:9d530d56a118 55
clynamen 0:9d530d56a118 56 /* Save FPU context here (if necessary) */
clynamen 0:9d530d56a118 57 /* Re-enable timer or interrupt here */
clynamen 0:9d530d56a118 58 /* Set model inputs here */
clynamen 0:9d530d56a118 59
clynamen 0:9d530d56a118 60 /* Step the model */
clynamen 0:9d530d56a118 61 Controller_step(Controller_M, Controller_U_distance, &Controller_Y_led);
clynamen 0:9d530d56a118 62
clynamen 0:9d530d56a118 63 /* Get model outputs here */
clynamen 0:9d530d56a118 64
clynamen 0:9d530d56a118 65 /* Indicate task complete */
clynamen 0:9d530d56a118 66 OverrunFlag = false;
clynamen 0:9d530d56a118 67
clynamen 0:9d530d56a118 68 /* Disable interrupts here */
clynamen 0:9d530d56a118 69 /* Restore FPU context here (if necessary) */
clynamen 0:9d530d56a118 70 /* Enable interrupts here */
clynamen 0:9d530d56a118 71 }
clynamen 0:9d530d56a118 72 //
clynamen 0:9d530d56a118 73 // End copy
clynamen 0:9d530d56a118 74 //
clynamen 0:9d530d56a118 75
clynamen 0:9d530d56a118 76 void do_step( void )
clynamen 0:9d530d56a118 77 {
clynamen 0:9d530d56a118 78 Controller_U_distance = distance;
clynamen 0:9d530d56a118 79
clynamen 0:9d530d56a118 80 rt_OneStep(Controller_M);
clynamen 0:9d530d56a118 81
clynamen 0:9d530d56a118 82 led = Controller_Y_led;
clynamen 0:9d530d56a118 83 }
clynamen 0:9d530d56a118 84
clynamen 0:9d530d56a118 85 void start( void )
clynamen 0:9d530d56a118 86 {
clynamen 0:9d530d56a118 87 t.start();
clynamen 0:9d530d56a118 88 }
clynamen 0:9d530d56a118 89
clynamen 0:9d530d56a118 90 void stop( void )
clynamen 0:9d530d56a118 91 {
clynamen 0:9d530d56a118 92 t.stop();
clynamen 0:9d530d56a118 93 distance = t.read_us()/58.0;
clynamen 0:9d530d56a118 94 t.reset();
clynamen 0:9d530d56a118 95 }
clynamen 0:9d530d56a118 96
clynamen 0:9d530d56a118 97
clynamen 0:9d530d56a118 98 Timer t1;
clynamen 0:9d530d56a118 99
clynamen 0:9d530d56a118 100 void serialSend(float v) {
clynamen 0:9d530d56a118 101 pc.printf("aaaa"); // header
clynamen 0:9d530d56a118 102 unsigned char* data = (unsigned char*) &v;
clynamen 0:9d530d56a118 103 for (int i = 0; i < 4; i++) {
clynamen 0:9d530d56a118 104 pc.putc(data[i]);
clynamen 0:9d530d56a118 105 }
clynamen 0:9d530d56a118 106 pc.printf("\r\n"); // end of line
clynamen 0:9d530d56a118 107 }
clynamen 0:9d530d56a118 108
clynamen 0:9d530d56a118 109 void serialSendVec(float vec[], int length) {
clynamen 0:9d530d56a118 110 pc.printf("aaaa"); // header
clynamen 0:9d530d56a118 111 unsigned char* data = (unsigned char*) vec;
clynamen 0:9d530d56a118 112 for (int i = 0; i < length*4; i++) {
clynamen 0:9d530d56a118 113 pc.putc(data[i]);
clynamen 0:9d530d56a118 114 }
clynamen 0:9d530d56a118 115 pc.printf("\r\n"); // end of line
clynamen 0:9d530d56a118 116 }
clynamen 0:9d530d56a118 117
clynamen 0:9d530d56a118 118 int main()
clynamen 0:9d530d56a118 119 {
clynamen 0:9d530d56a118 120
clynamen 0:9d530d56a118 121 float v = 0;
clynamen 0:9d530d56a118 122 int i = 0;
clynamen 0:9d530d56a118 123 float g = 0;
clynamen 0:9d530d56a118 124 while(true) {
clynamen 0:9d530d56a118 125 // pc.printf("sending at time %d\r\n", i++);
clynamen 0:9d530d56a118 126 serialSend(v);
clynamen 0:9d530d56a118 127 //pc.printf("%f\r\n", v);
clynamen 0:9d530d56a118 128
clynamen 0:9d530d56a118 129 v += 0.1f;
clynamen 0:9d530d56a118 130 g = sqrt(v);
clynamen 0:9d530d56a118 131 if(v > 10) v = 0;
clynamen 0:9d530d56a118 132 wait(0.001);
clynamen 0:9d530d56a118 133 led = !led;
clynamen 0:9d530d56a118 134 }
clynamen 0:9d530d56a118 135
clynamen 0:9d530d56a118 136 while(true) {
clynamen 0:9d530d56a118 137 char buf[200];
clynamen 0:9d530d56a118 138 t1.start();
clynamen 0:9d530d56a118 139 pc.printf("start read\r\n");
clynamen 0:9d530d56a118 140 pc.gets(buf, 199);
clynamen 0:9d530d56a118 141 t1.stop();
clynamen 0:9d530d56a118 142 buf[199]=0;
clynamen 0:9d530d56a118 143 float sp;
clynamen 0:9d530d56a118 144 sscanf(buf, "%f", &sp);
clynamen 0:9d530d56a118 145 pc.printf("received after %fs: <<%f>>\r\n", t1.read(), sp);
clynamen 0:9d530d56a118 146 }
clynamen 0:9d530d56a118 147 //
clynamen 0:9d530d56a118 148 // Copy from ert_main.c
clynamen 0:9d530d56a118 149 //
clynamen 0:9d530d56a118 150 /* Pack model data into RTM */
clynamen 0:9d530d56a118 151 Controller_M->ModelData.dwork = &Controller_DW;
clynamen 0:9d530d56a118 152
clynamen 0:9d530d56a118 153 /* Initialize model */
clynamen 0:9d530d56a118 154 Controller_initialize(Controller_M, &Controller_U_distance, &Controller_Y_led);
clynamen 0:9d530d56a118 155 //
clynamen 0:9d530d56a118 156 // End copy
clynamen 0:9d530d56a118 157 //
clynamen 0:9d530d56a118 158
clynamen 0:9d530d56a118 159 scheduler.attach( &do_step, 0.2 );
clynamen 0:9d530d56a118 160
clynamen 0:9d530d56a118 161 t.reset();
clynamen 0:9d530d56a118 162 echo.rise( &start );
clynamen 0:9d530d56a118 163 echo.fall( &stop );
clynamen 0:9d530d56a118 164
clynamen 0:9d530d56a118 165 Controller_U_distance = 0;
clynamen 0:9d530d56a118 166 trigger = 0;
clynamen 0:9d530d56a118 167
clynamen 0:9d530d56a118 168 while (true) {
clynamen 0:9d530d56a118 169 pc.printf( "Reading inputs....\n\r" );
clynamen 0:9d530d56a118 170
clynamen 0:9d530d56a118 171 trigger = 1;
clynamen 0:9d530d56a118 172 wait_us( 10 );
clynamen 0:9d530d56a118 173 trigger = 0;
clynamen 0:9d530d56a118 174
clynamen 0:9d530d56a118 175 pc.printf( "\n\rDistance: %.3f\n\r", Controller_U_distance );
clynamen 0:9d530d56a118 176
clynamen 0:9d530d56a118 177 }
clynamen 0:9d530d56a118 178 }