Diff: main.cpp

Revision:

40:04a05851ae7b

Parent:

39:ae41a212c170

Child:

41:d52445129908

--- a/main.cpp	Fri Mar 17 20:51:37 2017 +0000
+++ b/main.cpp	Fri Mar 17 22:19:09 2017 +0000
@@ -1,155 +1,54 @@
-#include "mbed.h"
-#include "rtos.h"
-#include "definitions.h"
-#include "motorControl.h"
-#include "parser.h"
-#include <cmath>
-
-volatile float w3 = 0;                  //Angular velocity
-volatile float duty = 0.5;
-volatile int count_i3 = 0;
-
-volatile int CHA_state = 0x00;
-volatile int CHB_state = 0x00;
-volatile int CH_state = 0x00;
-volatile int CH_state_prev = 0x00;
-
-volatile float diskPosition = 0.0;  //in degrees
-
-Timer dt_I3;
-Timer motorTimer;
-Ticker controlTicker;
-
-volatile float currentRevs = 0.0;                 //number of revs done
-volatile float goalRevs = 10.0;
-volatile float prevError = 0.0;
-volatile double dError = 0.0;
-volatile float currentError = 0.0;
-
-#define kp 0.012f
-#define kd 0.02f //0.5f
-#define k 10.0f
-#define dt 0.002f                        //given in ms, used to call the PID c.
-
-void control(){
-    if (w3 > 300) {
-        lead = 2;
-        return;
-    }
-    prevError = currentError;
-    currentRevs = diskPosition / 360 + count_i3;    // 1/360degr + 2pi*revs
-    currentError = goalRevs - currentRevs;
-    dError = (currentError - prevError)/dt;
-    duty = k*(kp*currentError + kd*dError);
-    if (duty > 0) lead = -2;
-    else {
-        lead = 2;
-        duty = -duty;
-    }
-}
-
-void i1rise(){
-    state = updateState();
-    motorOut((state-orState+lead+6)%6, duty);
-    
-    if (I3.read() == 1) {
-        count_i3++;
-    }    
-}
-
-void i3rise(){
-    state = updateState();
-    motorOut((state-orState+lead+6)%6, duty);
-    
-    w3 = angle/dt_I3.read();            //Calc angular velocity
-    
-    dt_I3.reset();
-    //count_i3++;
-}
-
-void i_edge(){
-   state = updateState();
-   motorOut((state-orState+lead+6)%6, duty);
-}
-
-void updateDiskPosition() {
-  if (CH_state != CH_state_prev) {
-    int diff = CH_state - CH_state_prev;
-    
-    CH_state_prev = CH_state;
-    if (abs(diff) == 1 || abs(diff) == 3) {
-        if (diff < 0)
-            diskPosition += angularResolution;
-        else
-            diskPosition -= angularResolution;
-    } 
-    else if (abs(diff) == 2) {
-        if (diff < 0)
-            diskPosition += 2.0f * angularResolution;
-        else
-            diskPosition -= 2.0f * angularResolution;
-    }
-
-    if (diskPosition >= 360.0f) {
-      diskPosition -= 360.0f;
-    } else if (diskPosition < -360.0f) {
-      diskPosition += 360.0f;
-    }
-  }
-}
-
-void updateRelativeState() {
-  CH_state = relativeStateMap[CHB_state + 2*CHA_state];
-}
-
-void CHA_rise() {
-  CHA_state = 1;
-  updateRelativeState();
-  updateDiskPosition();
-}
-void CHA_fall() {
-  CHA_state = 0;
-  updateRelativeState();
-  updateDiskPosition();
-}
-void CHB_rise() {
-  CHB_state = 1;
-  updateRelativeState();
-  updateDiskPosition();
-}
-void CHB_fall() {
-  CHB_state = 0;
-  updateRelativeState();
-  updateDiskPosition();
-}
-
-int main() {
-    motorHome();                        //Initialise motor before any interrupt
-    
-    dt_I3.start();                      //Start the time counters for velocity
-
-    controlTicker.attach(&control, dt);
-    
-    I1.rise(&i1rise);                   //Assign interrupt handlers for LEDs
-    I1.fall(&i_edge);
-    I2.rise(&i_edge);
-    I2.fall(&i_edge);
-    I3.rise(&i3rise);
-    I3.fall(&i_edge);
-    
-    CHA.rise(&CHA_rise);
-    CHA.fall(&CHA_fall);
-    CHB.rise(&CHB_rise);
-    CHB.fall(&CHB_fall);
-
-    state = updateState();
-    motorTimer.start();
-    motorOut((state-orState+lead+6)%6, 0.3f);            //Kickstart the motor
-    while (count_i3 <= goalRevs+1) {
-        //pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
-        pc.printf("Speed: %f, duty cycle: %f, revs done: %d, dError: %f , currentError: %f, prevError: %f, currentRevs: %f \n\r",w3, duty, count_i3, dError, currentError, prevError, currentRevs);
-        //pc.printf("Disk position: %f \n\r",fi0);
-    }
-    stopMotor();
-    return 0;
-}
\ No newline at end of file
+#include "mbed.h"
+#include "rtos.h"
+
+// Thread thread;
+//
+// volatile int counter = 0;
+//
+// void serialThread() {
+//   while(true) {
+//     // printf("Hey\n");
+//     counter++;
+//     Thread::wait(1000);
+//   }
+// }
+//
+// int main() {
+//   printf("Hello\n\r");
+//   thread.start(&serialThread);
+//
+//   while(true) {
+//     printf("%d\n\r", counter);
+//     Thread::wait(500);
+//     wait(5);
+//   }
+//
+//   return 0;
+// }
+// #include "mbed.h"
+// #include "rtos.h"
+ 
+Mutex stdio_mutex;
+ 
+void notify(const char* name, int state) {
+    stdio_mutex.lock();
+    printf("%s: %d\n\r", name, state);
+    stdio_mutex.unlock();
+}
+ 
+void test_thread(void const *args) {
+    while (true) {
+        notify((const char*)args, 0); Thread::wait(1000);
+        notify((const char*)args, 1); Thread::wait(1000);
+    }
+}
+ 
+int main() {
+    Thread t2;
+    Thread t3;
+ 
+    t2.start(callback(test_thread, (void *)"Th 2"));
+    t3.start(callback(test_thread, (void *)"Th 3"));
+ 
+    test_thread((void *)"Th 1");
+}