Diff: cube.cpp

Revision:

23:abe76b7c377a

Child:

24:c7b3bac429c5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/cube.cpp	Wed Apr 12 00:29:45 2017 +0000
@@ -0,0 +1,50 @@
+/*
+ * cube.h
+ * April 11, 2017
+ * 
+ * Control software for balancing cube senior design project
+ *
+ * Will Church
+ * Tom Rasmussen
+ */
+ 
+ #include "cube.h"
+
+/*
+ * Returns PWM duty cycle based on:
+ *  - wv wheel velocity
+ *  - ae angle error
+ *  - bv body velocity
+ *  - Kwv wheel vel gain
+ *  - Kbt angle gain
+ *  - Kbv body vel gain
+ */
+double calcPWM(config *c)
+{
+    // Converts and read the analog input value (value from 0.0 to 1.0):
+    double wv = c->hall->read();
+    wv = (wv - 2.0) * 5000.0; // Scale the velocity to rad/s
+
+    double bt = (*(c->angle) - c->eqAngle);
+
+    double r1 = (c->Kbt * bt + c->Kbv * *(c->vel) + c->Kwv * wv);
+
+    //Limit PWM range
+    if (r1 > 6.0) {r1 = 6.0;}
+    else if (r1 < -6.0) {r1 = -6.0;}
+        
+    // Normalize for PWM output
+    r1 = ((.4*(r1/6.0)) + 0.5);
+    
+    // Check if cube is too far tilted and send 0 torque
+    // May be redundant, check outer program
+    if (bt > (pi/8) || bt < -(pi/8)){
+        return .5;
+    }
+    return r1;
+}
+
+void updatePWM(config *c) {
+    c->pwm->write(calcPWM(c));
+}
+    
\ No newline at end of file