Bayley Wang / Mbed 2 deprecated priustroller_3

temp repo

Dependencies:   mbed

Fork of priustroller_2 by N K

Diff: core/motor.cpp

Revision:
30:2b6d426f3bfc
Parent:
29:cb03760ba9ea
Child:
33:e7b132029bae
--- a/core/motor.cpp	Sat Mar 14 23:42:46 2015 +0000
+++ b/core/motor.cpp	Sun Mar 15 01:45:22 2015 +0000
@@ -8,11 +8,13 @@
 }
 
 float Motor::GetCurrentC() {
-    return (float) global_ic / 4095.0f;
+    float x = (float) global_ic / 4095.0f;
+    return (x - _ic_zero) / _scale;
 }
 
 float Motor::GetCurrentB() {
-    return (float) global_ib / 4095.0f;
+    float x = (float) global_ib / 4095.0f;
+    return (x - _ib_zero) / _scale;
 }
 
 float Motor::GetPosition() {
@@ -26,4 +28,10 @@
 void Motor::Config(int num_poles, float kv) {
     _num_poles = num_poles;
     _kv = kv;
+}
+
+void Motor::InitSensors(float ib_zero, float ic_zero, float scale) {
+    _ib_zero = ib_zero;
+    _ic_zero = ic_zero;
+    _scale = scale;
 }
\ No newline at end of file