Diff: main.cpp

Revision:

41:d52445129908

Parent:

40:04a05851ae7b

Child:

42:2ee563cd6223

diff -r 04a05851ae7b -r d52445129908 main.cpp
--- a/main.cpp	Fri Mar 17 22:19:09 2017 +0000
+++ b/main.cpp	Fri Mar 17 22:27:14 2017 +0000
@@ -1,54 +1,155 @@
-#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");
-}
+#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