Revision 11:425dff6a4af9, committed 2015-03-25

Comitter:

tntmarket

Date:

Wed Mar 25 18:11:09 2015 +0000

Parent:

10:22c44650c7c1

Commit message:

Working

Changed in this revision

--- a/Control/Actuator.cpp	Wed Mar 25 16:36:11 2015 +0000
+++ b/Control/Actuator.cpp	Wed Mar 25 18:11:09 2015 +0000
@@ -26,13 +26,13 @@
    X[0]=T*(-0.1)   ; Y[0]=22  ;
    X[1]=T*(-0.099) ; Y[1]=22  ;
    X[2]=T*0.4      ; Y[2]=-8  ;
-   X[3]=T*0.401    ; Y[3]=-8  ;
+   X[3]=T*0.401    ; Y[3]=-8.07  ;
    X[4]=T*0.6      ; Y[4]=-19 ;
    X[5]=T*0.601    ; Y[5]=-19 ;
    X[6]=T*0.9      ; Y[6]=22  ;
    X[7]=T*0.901    ; Y[7]=22  ;
    X[8]=T*1.1      ; Y[8]=-8  ;
-   X[9]=T*1.101    ; Y[9]=-8  ;
+   X[9]=T*1.101    ; Y[9]=-8.07  ;
 
    hipCurve.set_points(X, Y);
 }

--- a/Control/control.cpp	Wed Mar 25 16:36:11 2015 +0000
+++ b/Control/control.cpp	Wed Mar 25 18:11:09 2015 +0000
@@ -10,17 +10,17 @@
 PaceDetector paceDetector;
 Actuator actuator;
 
-Sensor _sensor;
+Sensor* _sensor;
 
 void onStopToSwing() {
    paceDetector.lookForPeak();
-//   actuator.startHeelStrike(_sensor.time());
+   actuator.startHeelStrike(_sensor->time());
 }
 void onStopToMaybe() {
-//   actuator.startMidStance(_sensor.time());
+   actuator.startMidStance(_sensor->time());
 }
 
-void setup(Sensor &s) {
+void setup(Sensor* s) {
    _sensor = s;
    state.state = State::UNCALIBRATED;
    state.onSwing(&onStopToSwing);
@@ -29,9 +29,9 @@
 }
 
 void process() {
-   state.next(_sensor.thigh());
-   paceDetector.process(_sensor.time(), _sensor.thigh(), _sensor.shin());
-//   actuator.setPeriod(paceDetector.pace());
+   state.next(_sensor->thigh());
+   paceDetector.process(_sensor->time(), _sensor->thigh(), _sensor->shin());
+   actuator.setPeriod(paceDetector.pace());
 
    /* -actuator.getHip(_sensor.time()); */
    /* actuator.getKneeDiff(_sensor.time()); */
@@ -50,10 +50,10 @@
 }
 
 float getHip() {
-   return -actuator.getHip(_sensor.time());
+   return -actuator.getHip(_sensor->time());
 }
 
 float getKneeDiff() {
-   return actuator.getKneeDiff(_sensor.time());
+   return actuator.getKneeDiff(_sensor->time());
 }
 

--- a/Control/control.hpp	Wed Mar 25 16:36:11 2015 +0000
+++ b/Control/control.hpp	Wed Mar 25 18:11:09 2015 +0000
@@ -3,7 +3,7 @@
 
 #include "Sensor.hpp"
 
-void setup(Sensor &s);
+void setup(Sensor* s);
 void process();
 int getState();
 void setStateToStop();

--- a/main.cpp	Wed Mar 25 16:36:11 2015 +0000
+++ b/main.cpp	Wed Mar 25 18:11:09 2015 +0000
@@ -9,34 +9,29 @@
 Sensor sensor;
 DigitalOut btSwitch(PTE30);
 
-void IMU2_thread(void const *args)
-{
-    test_dmp2();
-    start_dmp2(mpu2);
-    while(1) {
-        update_dmp2();
-        Thread::yield();
-    }
-}
+
 void IMU_thread(void const *args)
 {
     test_dmp();
     start_dmp(mpu);
+    while(true) {
+        LED_ON
+        update_dmp();
+        Thread::yield();
+    }
+}
+void IMU2_thread(void const *args)
+{
     test_dmp2();
     start_dmp2(mpu2);
-    while(1) {
-        update_dmp();
+    while(true) {
+        LED_OFF
         update_dmp2();
         Thread::yield();
     }
 }
-
 void io_thread(void const *args)
 {
-    btSwitch = 1;
-    pc.baud(115200);
-    setup(sensor);
-
     while(true) {
         if(pc.readable() && pc.getc() == 'c') {
             sensor.calibrate();
@@ -45,11 +40,11 @@
         
         process();
         
-//        pc.printf(
-//            "%.3f;%.3f;%.3f;%i;%.3f;%.3f;%.3f;\r\n", 
-//            sensor.time(), sensor.thigh(), sensor.shin(), getState(), getPace(), 0, 0
-//        );
-        pc.printf("crap\r\n", 0);
+        pc.printf(
+            "%.3f;%.3f;%.3f;%i;%.3f;%.3f;%.3f;\r\n",
+            sensor.time(), sensor.thigh(), sensor.shin(), getState(), getPace(), getHip(), getKneeDiff()
+        );
+
         Thread::wait(10);
     }
 }
@@ -65,10 +60,15 @@
 int main()
 {
     t.start();
+    
+    btSwitch = 1;
+    pc.baud(115200);
+    
+    setup(&sensor);
 
-    Thread bt_shell_th(io_thread, NULL, osPriorityNormal, 2048, NULL);
-    Thread IMU_th(IMU_thread, NULL, osPriorityNormal, 2048, NULL);
-    Thread IMU2_th(IMU2_thread, NULL, osPriorityNormal, 2048, NULL);
+    Thread bt_shell_th(io_thread, NULL, osPriorityNormal, 1024, NULL);
+    Thread IMU_th(IMU_thread, NULL, osPriorityNormal, 768, NULL);
+    Thread IMU2_th(IMU2_thread, NULL, osPriorityNormal, 768, NULL);
 //    Thread control_th(control_thread, NULL, osPriorityNormal, 2548, NULL);
 
     Thread::wait(osWaitForever);