Diff: main.cpp

Revision:

0:9d530d56a118

Child:

1:45911e86ffee

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jul 24 14:42:26 2016 +0000
@@ -0,0 +1,178 @@
+#include "mbed.h"
+
+  #include "controller.h"
+  #include "rtwtypes.h"
+
+DigitalOut      led(LED_RED);
+DigitalOut      trigger(D2);
+InterruptIn     echo(D4);
+Timer           t;
+Ticker          scheduler;
+
+float           distance;
+
+Serial  pc(USBTX, USBRX); // tx, rx
+
+
+// 
+// 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;
+
+/*
+   * 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 */
+  
+    /* 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 )
+{
+    t.start();
+}
+
+void stop( void )
+{
+    t.stop();
+    distance = t.read_us()/58.0;
+    t.reset();
+}
+
+ 
+Timer t1;
+
+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]);
+    }
+    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]);
+    }
+    pc.printf("\r\n"); // end of line
+}
+    
+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);
+  
+        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);
+    }
+    //
+    // 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" );
+
+        trigger = 1;
+        wait_us( 10 );
+        trigger = 0;
+
+        pc.printf( "\n\rDistance: %.3f\n\r", Controller_U_distance );   
+
+    }
+}
\ No newline at end of file