Diff: main.cpp

Revision:

0:74a5723d604a

Child:

2:fe637a5f3387

diff -r 000000000000 -r 74a5723d604a main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 13 15:42:04 2017 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "definitions.h"
+#include "motorControl.h"
+
+volatile uint8_t state = 0;
+volatile uint8_t orState = 0;                   //Motor rotor offset.
+volatile float w [3] = {0, 0, 0};               //Angular velocities
+volatile float avgW = 0;
+
+const uint16_t angle = 6283;                    //2*pi*1000 for 1 revolution
+Timer dt_I1;
+Timer dt_I2;
+Timer dt_I3;
+Timer motorTimer;
+
+void i1rise(){
+    state = updateState();
+    motorOut((state-orState+lead+6)%6);
+    
+    dt_I1.stop();
+    w[0] = angle/dt_I1.read_ms();                //Calc angular velocity
+    
+    dt_I1.reset();
+    dt_I1.start();                       
+}
+
+void i2rise(){
+    state = updateState();
+    motorOut((state-orState+lead+6)%6);
+    
+    dt_I2.stop();
+    w[1] = angle/dt_I2.read_ms();
+    
+    dt_I2.reset();
+    dt_I2.start();                       
+}
+
+void i3rise(){
+    state = updateState();
+    motorOut((state-orState+lead+6)%6);
+    
+    dt_I3.stop();
+    w[2] = angle/dt_I3.read_ms();
+    
+    dt_I3.reset();
+    dt_I3.start();                       
+}
+
+void i_fall(){
+   state = updateState();
+   motorOut((state-orState+lead+6)%6);
+}  
+
+void CHA_rise(){
+}
+void CHA_fall(){
+}
+void CHB_rise(){
+}
+void CHB_fall(){
+}
+
+int main() {
+    //Probably measure orState from hardware and make it a const?
+    orState = motorHome();  //Initialise motor before any interrupt
+    
+    dt_I1.start();          //Start the time counters for velocity
+    dt_I2.start();          //Probably put these in an init function?
+    dt_I3.start();
+    
+    motorOut(4);            //Kickstart the motor
+    motorTimer.start();
+    
+    I1.rise(&i1rise);       //Assign interrupt handlers for LEDs
+    I1.fall(&i_fall);
+    I2.rise(&i2rise);
+    I2.fall(&i_fall);
+    I3.rise(&i3rise);
+    I3.fall(&i_fall);
+//    CHA.rise(&CHA_rise);
+//    CHA.fall(&CHA_fall);
+//    CHB.rise(&CHB_rise);
+//    CHB.fall(&CHB_fall);
+
+    while (1) {
+        avgW = (w[0] + w[1] + w[2])/3;    //average speeds for better prediction
+        if(motorTimer.read_ms() >= 10000) {
+            stopMotor();
+            return 0;    
+        }
+    }
+}
\ No newline at end of file