File content as of revision 4:778bc352c47f:

#include "Axis.h"

extern BufferedSerial buffered_pc;

Axis::Axis(ODrive* od, int ax, DigitalIn* homeSwitch, calVals calibration_,axisName indentity)
{
    odrive = od;
    axNum = ax;
    homeSwitch_ = homeSwitch;
    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());
}