Diff: Axis.cpp

Revision:

4:778bc352c47f

Parent:

3:10fa3102c2d7

Child:

5:01e1e68309ae

--- a/Axis.cpp	Fri Oct 05 15:57:55 2018 +0000
+++ b/Axis.cpp	Sun Oct 07 19:40:12 2018 +0000
@@ -1,22 +1,79 @@
 
 #include "Axis.h"
 
-void  Axis::Axis(&ODrive od, int ax, DigitalIn homeSwitch,float gearRatio)
+extern BufferedSerial buffered_pc;
+
+Axis::Axis(ODrive* od, int ax, DigitalIn* homeSwitch, calVals calibration_,axisName indentity)
 {
     odrive = od;
     axNum = ax;
     homeSwitch_ = homeSwitch;
-    gearRatio_ = gearRatio;
+    switch(indentity) {
+        case AX_A:
+            rotation_offset = calibration_.Aoffset;
+            break;
+        case AX_B:
+            rotation_offset = calibration_.Boffset;
+            break;
+        case AX_C:
+            rotation_offset = calibration_.Coffset;
+            break;
+    }
+
+    gearRatio_ = calibration_.gearRatio; // 89/24
+    pulse_per_rev = 8192 * gearRatio_;
+    pulse_per_rad = pulse_per_rev / (2* 3.14159265359);
+
 }
 
 void  Axis::homeAxis()
 {
+    int count  = 0;
+    bool homed = false;
+    homeOffset = 0;
+    int homeCount = 0;
+    while(!homed) {
+        odrive->SetPosition(axNum,homeOffset);
+        Thread::wait(1);
+        for(int i = 0 ; i < 99; i++) {
+            if(*homeSwitch_ == false) {
+                homeCount++;
+            } else {
+                homeCount = 0;
+            }
+            if(homeCount > 50) {
+                homed = true;
+            }
+        }
+        homeCount= 0;
+        homeOffset++;
+    }
+    homeOffset--;
+    float homeBounce = 0.5;
+    goAngle(homeBounce);
+    currentSetPos = 0.1;
+    currentSetVel = 0;
 }
 
 void  Axis::goAngle(float angle)
 {
+    odrive->SetPosition(axNum,homeOffset - angle*pulse_per_rad - rotation_offset*pulse_per_rad);
 }
 
 void  Axis::goAngleSpeed(float angle, float speed)
 {
+    odrive->SetPosition(axNum,homeOffset - angle*pulse_per_rad - rotation_offset*pulse_per_rad, -speed*pulse_per_rad);
+}
+
+void Axis::runState(int requestedState)
+{
+    odrive->run_state(axNum, requestedState, false); // don't wait
+}
+
+void Axis::setMaxVel(float stepsPerSec)
+{
+    std::stringstream ss;
+    ss<< "w axis" << axNum << ".controller.config.vel_limit " << stepsPerSec << '\n';
+    buffered_pc.printf(ss.str().c_str());
+    odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
 }
\ No newline at end of file