Diff: main.cpp

Revision:

4:dc705df93090

Parent:

2:fe637a5f3387

Child:

7:6bf4a61cf7c7

diff -r fe637a5f3387 -r dc705df93090 main.cpp
--- a/main.cpp	Mon Mar 13 17:12:25 2017 +0000
+++ b/main.cpp	Tue Mar 14 13:09:53 2017 +0000
@@ -3,17 +3,40 @@
 #include "definitions.h"
 #include "motorControl.h"
 
+#define kp 0.75
+#define ki 0.5
+#define kd 1
+#define dt 0.02 //given in ms, used to call a ticker
+
 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;
-volatile int duty = 0;
+volatile int duty = 100;
+volatile int count_i3 = 0;
 
 const uint16_t angle = 6283;                    //2*pi*1000 for 1 revolution
 Timer dt_I1;
 Timer dt_I2;
 Timer dt_I3;
 Timer motorTimer;
+Ticker controlTicker;
+
+volatile float fi0 = 0;     //number of revs done
+volatile int goalRevs = 20;
+volatile float fi = 2*3.1415*goalRevs;
+volatile float goalW = 0;      //desired angular velocity
+volatile int accError = 0;
+
+/*
+void control(){
+    fi0 = 6.283 * count_i3; //fi0 = 2*pi*revs
+    int error = fi - fi0;
+    accError += error;
+    dError = (prevError - error)/dt;    //prev error needs to be stored
+    goalW = kp*error + ki*accError + kd*dError;
+}
+*/
 
 void i1rise(){
     state = updateState();
@@ -45,7 +68,8 @@
     w[2] = angle/dt_I3.read_ms();
     
     dt_I3.reset();
-    dt_I3.start();                       
+    dt_I3.start();
+    count_i3++;                   
 }
 
 void i_fall(){
@@ -73,6 +97,8 @@
     motorOut(4, 100);            //Kickstart the motor
     motorTimer.start();
     
+    //controlTicker.attach(&control, dt);
+    
     I1.rise(&i1rise);       //Assign interrupt handlers for LEDs
     I1.fall(&i_fall);
     I2.rise(&i2rise);
@@ -85,13 +111,24 @@
 //    CHB.fall(&CHB_fall);
 
     while (1) {
-        duty += 5;
+        /*
+        const int rotations = 10;
+        pc.printf("Rotation: %d ",count_i3);
+        if (count_i3 == rotations) {
+            stopMotor();
+            return 0;
+        }
+        */
+        /*
+        avgW = (w[0] + w[1] + w[2])/3;    //average speeds for better prediction
+        pc.printf("Speed: %f, duty cycle: %d ; ",w[2], duty);
+        duty ++;
         wait(2);
-        if(duty == 100) {
+        if(duty > 100) {
             stopMotor();
             return 0;    
         }
-        avgW = (w[0] + w[1] + w[2])/3;    //average speeds for better prediction
+        */
         //if(motorTimer.read_ms() >= 10000) {
         //    stopMotor();
         //    return 0;