Diff: main.cpp

Revision:

1:45911e86ffee

Parent:

0:9d530d56a118

Child:

2:5811e080f41d

--- a/main.cpp	Sun Jul 24 14:42:26 2016 +0000
+++ b/main.cpp	Tue Jul 26 20:15:23 2016 +0000
@@ -1,15 +1,19 @@
 #include "mbed.h"
 
-  #include "controller.h"
-  #include "rtwtypes.h"
+#include "rtwtypes.h"
+#include "serialdata.h"
 
-DigitalOut      led(LED_RED);
+DigitalOut red(LED_RED);
+DigitalOut green(LED_GREEN);
+DigitalOut blue(LED_BLUE);
+
 DigitalOut      trigger(D2);
 InterruptIn     echo(D4);
 Timer           t;
 Ticker          scheduler;
 
-float           distance;
+volatile float           distance;
+float fault = 0;
 
 Serial  pc(USBTX, USBRX); // tx, rx
 
@@ -17,70 +21,61 @@
 // 
 // Copy from ert_main.c
 //
-static RT_MODEL_Controller_T Controller_M_;
-static RT_MODEL_Controller_T *const Controller_M = &Controller_M_;/* Real-time model */
-static DW_Controller_T Controller_DW;  /* Observable states */
-  
-/* '<Root>/distance' */
-static real32_T Controller_U_distance;
-  
-/* '<Root>/led' */
-static uint8_T Controller_Y_led;
+
+#include <stddef.h>
+#include <stdio.h>                     /* This ert_main.c example uses printf/fflush */
+#include "AEB0.h"                      /* Model's header file */
+#include "rtwtypes.h"
 
 /*
-   * Associating rt_OneStep with a real-time clock or interrupt service routine
-   * is what makes the generated code "real-time".  The function rt_OneStep is
-   * always associated with the base rate of the model.  Subrates are managed
-   * by the base rate from inside the generated code.  Enabling/disabling
-   * interrupts and floating point context switches are target specific.  This
-   * example code indicates where these should take place relative to executing
-   * the generated code step function.  Overrun behavior should be tailored to
-   * your application needs.  This example simply sets an error status in the
-   * real-time model and returns from rt_OneStep.
-   */
-  void rt_OneStep(RT_MODEL_Controller_T *const Controller_M);
-  void rt_OneStep(RT_MODEL_Controller_T *const Controller_M)
-  {
-    static boolean_T OverrunFlag = false;
-  
-    /* Disable interrupts here */
+ * Associating rt_OneStep with a real-time clock or interrupt service routine
+ * is what makes the generated code "real-time".  The function rt_OneStep is
+ * always associated with the base rate of the model.  Subrates are managed
+ * by the base rate from inside the generated code.  Enabling/disabling
+ * interrupts and floating point context switches are target specific.  This
+ * example code indicates where these should take place relative to executing
+ * the generated code step function.  Overrun behavior should be tailored to
+ * your application needs.  This example simply sets an error status in the
+ * real-time model and returns from rt_OneStep.
+ */
+void rt_OneStep(void);
+void rt_OneStep(void)
+{
+  static boolean_T OverrunFlag = false;
+
+  /* Disable interrupts here */
+
+  /* Check for overrun */
+  if (OverrunFlag) {
+    rtmSetErrorStatus(AEB0_M, "Overrun");
+    return;
+  }
+
+  OverrunFlag = true;
+
+  /* Save FPU context here (if necessary) */
+  /* Re-enable timer or interrupt here */
+  /* Set model inputs here */
+
+  /* Step the model */
+  AEB0_step();
+
+  /* Get model outputs here */
+
+  /* Indicate task complete */
+  OverrunFlag = false;
+
+  /* Disable interrupts here */
+  /* Restore FPU context here (if necessary) */
+  /* Enable interrupts here */
+}
+
   
-    /* Check for overrun */
-    if (OverrunFlag) {
-      rtmSetErrorStatus(Controller_M, "Overrun");
-      return;
-    }
-  
-    OverrunFlag = true;
-  
-    /* Save FPU context here (if necessary) */
-    /* Re-enable timer or interrupt here */
-    /* Set model inputs here */
-  
-    /* Step the model */
-    Controller_step(Controller_M, Controller_U_distance, &Controller_Y_led);
-  
-    /* Get model outputs here */
-  
-    /* Indicate task complete */
-    OverrunFlag = false;
-  
-    /* Disable interrupts here */
-    /* Restore FPU context here (if necessary) */
-    /* Enable interrupts here */
-  } 
 //
 // End copy
 //
 
-void    do_step( void )
-{
-    Controller_U_distance = distance;
-    
-    rt_OneStep(Controller_M);
-    
-    led = Controller_Y_led;
-}
+
     
 void    start( void )
 {
@@ -91,88 +86,84 @@
 {
     t.stop();
     distance = t.read_us()/58.0;
+    distance /= 2; // from 0 to 45m distance;
+    if(distance>150) distance = 150;
     t.reset();
 }
 
  
-Timer t1;
+extern ExtU_AEB0_T AEB0_U;
+extern ExtY_AEB0_T AEB0_Y;
+
+enum color {
+    NONE, RED, GREEN, BLUE
+};
+
+enum {
+    LED_ON = 0,
+    LED_OFF = 1
+};
 
-void serialSend(float v) {
-    pc.printf("aaaa"); // header
-    unsigned char* data = (unsigned char*) &v;
-    for (int i = 0; i < 4; i++) {
-      pc.putc(data[i]);
+void setColor(color c) {
+    red = LED_OFF;
+    blue = LED_OFF;
+    green = LED_OFF;
+    switch(c) {
+        case NONE:
+            break;
+        case RED:
+            red = LED_ON;
+            break;
+        case BLUE:
+            blue = LED_ON;
+            break;
+        case GREEN:
+            green = LED_ON;
+            break;
     }
-    pc.printf("\r\n"); // end of line
 }
 
-void serialSendVec(float vec[], int length) {
-    pc.printf("aaaa"); // header
-    unsigned char* data = (unsigned char*) vec;
-    for (int i = 0; i < length*4; i++) {
-      pc.putc(data[i]);
+void    do_step( void )
+{
+    AEB0_U.speed_ms = serialRecv();
+    AEB0_U.distance_m = distance;
+//    
+ 
+    rt_OneStep();
+    float brake = AEB0_Y.brake;
+    fault = AEB0_Y.fault;
+    if(fault) {
+        setColor(RED);
     }
-    pc.printf("\r\n"); // end of line
+    float data[3] = {brake, distance, fault};
+    serialSendVec(data, 3);
 }
     
 int main()
 {
     
-    float v = 0;
-    int i = 0;
-    float g = 0;
-    while(true) {
-       // pc.printf("sending at time %d\r\n", i++);
-        serialSend(v);
-    //pc.printf("%f\r\n", v);
+    AEB0_initialize();
   
-        v += 0.1f;
-        g = sqrt(v);
-        if(v > 10) v = 0;
-        wait(0.001);
-        led = !led;
-    }
-    
-    while(true) {
-        char buf[200];
-        t1.start();
-        pc.printf("start read\r\n");
-        pc.gets(buf, 199);
-        t1.stop();
-        buf[199]=0;
-        float sp;
-        sscanf(buf, "%f", &sp);
-        pc.printf("received after %fs: <<%f>>\r\n", t1.read(), sp);
-    }
+    scheduler.attach( &do_step, 0.2 );
+    setColor(NONE);
     //
-    // Copy from ert_main.c
-    //    
-    /* Pack model data into RTM */
-    Controller_M->ModelData.dwork = &Controller_DW;
-  
-    /* Initialize model */
-    Controller_initialize(Controller_M, &Controller_U_distance, &Controller_Y_led);
-    //
-    // End copy
-    //
-
-    scheduler.attach( &do_step, 0.2 );
-    
     t.reset();
     echo.rise( &start );
     echo.fall( &stop );
- 
-    Controller_U_distance = 0;
+
     trigger = 0;
       
     while (true) {
-        pc.printf( "Reading inputs....\n\r" );
+      //  pc.printf( "Reading inputs....\n\r" );
+
 
         trigger = 1;
         wait_us( 10 );
         trigger = 0;
 
-        pc.printf( "\n\rDistance: %.3f\n\r", Controller_U_distance );   
 
     }
+    
+    AEB0_terminate();
+    return 0;
 }
\ No newline at end of file