joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

Axis.cpp@4:778bc352c47f, 2018-10-07 (annotated)

Committer:
je310
Date:
Sun Oct 07 19:40:12 2018 +0000
Revision:
4:778bc352c47f
Parent:
3:10fa3102c2d7
Child:
5:01e1e68309ae
can work smoothly, though voltage reading not working;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
je310 3:10fa3102c2d7 1
je310 3:10fa3102c2d7 2 #include "Axis.h"
je310 3:10fa3102c2d7 3
je310 4:778bc352c47f 4 extern BufferedSerial buffered_pc;
je310 4:778bc352c47f 5
je310 4:778bc352c47f 6 Axis::Axis(ODrive* od, int ax, DigitalIn* homeSwitch, calVals calibration_,axisName indentity)
je310 3:10fa3102c2d7 7 {
je310 3:10fa3102c2d7 8 odrive = od;
je310 3:10fa3102c2d7 9 axNum = ax;
je310 3:10fa3102c2d7 10 homeSwitch_ = homeSwitch;
je310 4:778bc352c47f 11 switch(indentity) {
je310 4:778bc352c47f 12 case AX_A:
je310 4:778bc352c47f 13 rotation_offset = calibration_.Aoffset;
je310 4:778bc352c47f 14 break;
je310 4:778bc352c47f 15 case AX_B:
je310 4:778bc352c47f 16 rotation_offset = calibration_.Boffset;
je310 4:778bc352c47f 17 break;
je310 4:778bc352c47f 18 case AX_C:
je310 4:778bc352c47f 19 rotation_offset = calibration_.Coffset;
je310 4:778bc352c47f 20 break;
je310 4:778bc352c47f 21 }
je310 4:778bc352c47f 22
je310 4:778bc352c47f 23 gearRatio_ = calibration_.gearRatio; // 89/24
je310 4:778bc352c47f 24 pulse_per_rev = 8192 * gearRatio_;
je310 4:778bc352c47f 25 pulse_per_rad = pulse_per_rev / (2* 3.14159265359);
je310 4:778bc352c47f 26
je310 3:10fa3102c2d7 27 }
je310 3:10fa3102c2d7 28
je310 3:10fa3102c2d7 29 void Axis::homeAxis()
je310 3:10fa3102c2d7 30 {
je310 4:778bc352c47f 31 int count = 0;
je310 4:778bc352c47f 32 bool homed = false;
je310 4:778bc352c47f 33 homeOffset = 0;
je310 4:778bc352c47f 34 int homeCount = 0;
je310 4:778bc352c47f 35 while(!homed) {
je310 4:778bc352c47f 36 odrive->SetPosition(axNum,homeOffset);
je310 4:778bc352c47f 37 Thread::wait(1);
je310 4:778bc352c47f 38 for(int i = 0 ; i < 99; i++) {
je310 4:778bc352c47f 39 if(*homeSwitch_ == false) {
je310 4:778bc352c47f 40 homeCount++;
je310 4:778bc352c47f 41 } else {
je310 4:778bc352c47f 42 homeCount = 0;
je310 4:778bc352c47f 43 }
je310 4:778bc352c47f 44 if(homeCount > 50) {
je310 4:778bc352c47f 45 homed = true;
je310 4:778bc352c47f 46 }
je310 4:778bc352c47f 47 }
je310 4:778bc352c47f 48 homeCount= 0;
je310 4:778bc352c47f 49 homeOffset++;
je310 4:778bc352c47f 50 }
je310 4:778bc352c47f 51 homeOffset--;
je310 4:778bc352c47f 52 float homeBounce = 0.5;
je310 4:778bc352c47f 53 goAngle(homeBounce);
je310 4:778bc352c47f 54 currentSetPos = 0.1;
je310 4:778bc352c47f 55 currentSetVel = 0;
je310 3:10fa3102c2d7 56 }
je310 3:10fa3102c2d7 57
je310 3:10fa3102c2d7 58 void Axis::goAngle(float angle)
je310 3:10fa3102c2d7 59 {
je310 4:778bc352c47f 60 odrive->SetPosition(axNum,homeOffset - angle*pulse_per_rad - rotation_offset*pulse_per_rad);
je310 3:10fa3102c2d7 61 }
je310 3:10fa3102c2d7 62
je310 3:10fa3102c2d7 63 void Axis::goAngleSpeed(float angle, float speed)
je310 3:10fa3102c2d7 64 {
je310 4:778bc352c47f 65 odrive->SetPosition(axNum,homeOffset - angle*pulse_per_rad - rotation_offset*pulse_per_rad, -speed*pulse_per_rad);
je310 4:778bc352c47f 66 }
je310 4:778bc352c47f 67
je310 4:778bc352c47f 68 void Axis::runState(int requestedState)
je310 4:778bc352c47f 69 {
je310 4:778bc352c47f 70 odrive->run_state(axNum, requestedState, false); // don't wait
je310 4:778bc352c47f 71 }
je310 4:778bc352c47f 72
je310 4:778bc352c47f 73 void Axis::setMaxVel(float stepsPerSec)
je310 4:778bc352c47f 74 {
je310 4:778bc352c47f 75 std::stringstream ss;
je310 4:778bc352c47f 76 ss<< "w axis" << axNum << ".controller.config.vel_limit " << stepsPerSec << '\n';
je310 4:778bc352c47f 77 buffered_pc.printf(ss.str().c_str());
je310 4:778bc352c47f 78 odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
je310 3:10fa3102c2d7 79 }