Delta Robot example
Dependencies: BufferedSerial Eigen
Fork of TCPSocket_Example by
Axis.cpp@5:01e1e68309ae, 2018-10-15 (annotated)
- Committer:
- je310
- Date:
- Mon Oct 15 18:30:20 2018 +0000
- Revision:
- 5:01e1e68309ae
- Parent:
- 4:778bc352c47f
testing eigen;
Who changed what in which revision?
User | Revision | Line number | New 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 | 5:01e1e68309ae | 6 | Axis::Axis(ODrive* od, int ax, DigitalIn* homeSwitch, calVals calibration_,axisName identity) |
je310 | 3:10fa3102c2d7 | 7 | { |
je310 | 3:10fa3102c2d7 | 8 | odrive = od; |
je310 | 3:10fa3102c2d7 | 9 | axNum = ax; |
je310 | 3:10fa3102c2d7 | 10 | homeSwitch_ = homeSwitch; |
je310 | 5:01e1e68309ae | 11 | name = identity; |
je310 | 5:01e1e68309ae | 12 | switch(identity) { |
je310 | 4:778bc352c47f | 13 | case AX_A: |
je310 | 4:778bc352c47f | 14 | rotation_offset = calibration_.Aoffset; |
je310 | 4:778bc352c47f | 15 | break; |
je310 | 4:778bc352c47f | 16 | case AX_B: |
je310 | 4:778bc352c47f | 17 | rotation_offset = calibration_.Boffset; |
je310 | 4:778bc352c47f | 18 | break; |
je310 | 4:778bc352c47f | 19 | case AX_C: |
je310 | 4:778bc352c47f | 20 | rotation_offset = calibration_.Coffset; |
je310 | 4:778bc352c47f | 21 | break; |
je310 | 4:778bc352c47f | 22 | } |
je310 | 4:778bc352c47f | 23 | |
je310 | 4:778bc352c47f | 24 | gearRatio_ = calibration_.gearRatio; // 89/24 |
je310 | 4:778bc352c47f | 25 | pulse_per_rev = 8192 * gearRatio_; |
je310 | 4:778bc352c47f | 26 | pulse_per_rad = pulse_per_rev / (2* 3.14159265359); |
je310 | 4:778bc352c47f | 27 | |
je310 | 3:10fa3102c2d7 | 28 | } |
je310 | 3:10fa3102c2d7 | 29 | |
je310 | 3:10fa3102c2d7 | 30 | void Axis::homeAxis() |
je310 | 3:10fa3102c2d7 | 31 | { |
je310 | 4:778bc352c47f | 32 | int count = 0; |
je310 | 4:778bc352c47f | 33 | bool homed = false; |
je310 | 4:778bc352c47f | 34 | homeOffset = 0; |
je310 | 4:778bc352c47f | 35 | int homeCount = 0; |
je310 | 4:778bc352c47f | 36 | while(!homed) { |
je310 | 4:778bc352c47f | 37 | odrive->SetPosition(axNum,homeOffset); |
je310 | 4:778bc352c47f | 38 | Thread::wait(1); |
je310 | 4:778bc352c47f | 39 | for(int i = 0 ; i < 99; i++) { |
je310 | 4:778bc352c47f | 40 | if(*homeSwitch_ == false) { |
je310 | 4:778bc352c47f | 41 | homeCount++; |
je310 | 4:778bc352c47f | 42 | } else { |
je310 | 4:778bc352c47f | 43 | homeCount = 0; |
je310 | 4:778bc352c47f | 44 | } |
je310 | 4:778bc352c47f | 45 | if(homeCount > 50) { |
je310 | 4:778bc352c47f | 46 | homed = true; |
je310 | 4:778bc352c47f | 47 | } |
je310 | 4:778bc352c47f | 48 | } |
je310 | 4:778bc352c47f | 49 | homeCount= 0; |
je310 | 4:778bc352c47f | 50 | homeOffset++; |
je310 | 4:778bc352c47f | 51 | } |
je310 | 4:778bc352c47f | 52 | homeOffset--; |
je310 | 4:778bc352c47f | 53 | float homeBounce = 0.5; |
je310 | 4:778bc352c47f | 54 | goAngle(homeBounce); |
je310 | 4:778bc352c47f | 55 | currentSetPos = 0.1; |
je310 | 4:778bc352c47f | 56 | currentSetVel = 0; |
je310 | 3:10fa3102c2d7 | 57 | } |
je310 | 3:10fa3102c2d7 | 58 | |
je310 | 5:01e1e68309ae | 59 | int Axis::readState(){ |
je310 | 5:01e1e68309ae | 60 | std::stringstream ss; |
je310 | 5:01e1e68309ae | 61 | ss<< "r axis" << axNum << ".current_state" << '\n'; |
je310 | 5:01e1e68309ae | 62 | //buffered_pc.printf(ss.str().c_str()); |
je310 | 5:01e1e68309ae | 63 | odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length()); |
je310 | 5:01e1e68309ae | 64 | return odrive->readInt(); |
je310 | 5:01e1e68309ae | 65 | } |
je310 | 5:01e1e68309ae | 66 | |
je310 | 5:01e1e68309ae | 67 | void Axis::findIndex() |
je310 | 5:01e1e68309ae | 68 | { |
je310 | 5:01e1e68309ae | 69 | |
je310 | 5:01e1e68309ae | 70 | |
je310 | 5:01e1e68309ae | 71 | int requested_state; |
je310 | 5:01e1e68309ae | 72 | requested_state = ODrive::AXIS_STATE_ENCODER_INDEX_SEARCH; |
je310 | 5:01e1e68309ae | 73 | runState(requested_state); |
je310 | 5:01e1e68309ae | 74 | int foundIndex = 0; |
je310 | 5:01e1e68309ae | 75 | while(!foundIndex) { |
je310 | 5:01e1e68309ae | 76 | Thread::wait(10); |
je310 | 5:01e1e68309ae | 77 | int state = readState(); |
je310 | 5:01e1e68309ae | 78 | if( state == ODrive::AXIS_STATE_IDLE) foundIndex = 1; |
je310 | 5:01e1e68309ae | 79 | } |
je310 | 5:01e1e68309ae | 80 | int requestedState; |
je310 | 5:01e1e68309ae | 81 | requestedState = ODrive::AXIS_STATE_CLOSED_LOOP_CONTROL; |
je310 | 5:01e1e68309ae | 82 | runState(requestedState); |
je310 | 5:01e1e68309ae | 83 | } |
je310 | 5:01e1e68309ae | 84 | |
je310 | 5:01e1e68309ae | 85 | void Axis::idle() |
je310 | 5:01e1e68309ae | 86 | { |
je310 | 5:01e1e68309ae | 87 | int requested_state; |
je310 | 5:01e1e68309ae | 88 | requested_state = ODrive::AXIS_STATE_IDLE; |
je310 | 5:01e1e68309ae | 89 | runState(requested_state); |
je310 | 5:01e1e68309ae | 90 | } |
je310 | 5:01e1e68309ae | 91 | |
je310 | 3:10fa3102c2d7 | 92 | void Axis::goAngle(float angle) |
je310 | 3:10fa3102c2d7 | 93 | { |
je310 | 4:778bc352c47f | 94 | odrive->SetPosition(axNum,homeOffset - angle*pulse_per_rad - rotation_offset*pulse_per_rad); |
je310 | 3:10fa3102c2d7 | 95 | } |
je310 | 3:10fa3102c2d7 | 96 | |
je310 | 3:10fa3102c2d7 | 97 | void Axis::goAngleSpeed(float angle, float speed) |
je310 | 3:10fa3102c2d7 | 98 | { |
je310 | 4:778bc352c47f | 99 | odrive->SetPosition(axNum,homeOffset - angle*pulse_per_rad - rotation_offset*pulse_per_rad, -speed*pulse_per_rad); |
je310 | 4:778bc352c47f | 100 | } |
je310 | 4:778bc352c47f | 101 | |
je310 | 4:778bc352c47f | 102 | void Axis::runState(int requestedState) |
je310 | 4:778bc352c47f | 103 | { |
je310 | 4:778bc352c47f | 104 | odrive->run_state(axNum, requestedState, false); // don't wait |
je310 | 4:778bc352c47f | 105 | } |
je310 | 4:778bc352c47f | 106 | |
je310 | 5:01e1e68309ae | 107 | int Axis::setParams(float stepsPerSec, float vel_gain, float encoder_bandwidth, float pos_gain, float vel_int) |
je310 | 5:01e1e68309ae | 108 | { |
je310 | 5:01e1e68309ae | 109 | int error = 0; |
je310 | 5:01e1e68309ae | 110 | error += writeParam(".controller.config.vel_limit", stepsPerSec); |
je310 | 5:01e1e68309ae | 111 | error +=writeParam(".controller.config.vel_gain", vel_gain); |
je310 | 5:01e1e68309ae | 112 | error +=writeParam(".controller.config.pos_gain", pos_gain); |
je310 | 5:01e1e68309ae | 113 | error +=writeParam(".encoder.config.bandwidth", encoder_bandwidth); |
je310 | 5:01e1e68309ae | 114 | error +=writeParam(".controller.config.vel_integrator_gain", vel_int); |
je310 | 5:01e1e68309ae | 115 | return error; |
je310 | 5:01e1e68309ae | 116 | |
je310 | 5:01e1e68309ae | 117 | // std::stringstream ss; |
je310 | 5:01e1e68309ae | 118 | // ss<< "w axis" << axNum << ".controller.config.vel_limit " << stepsPerSec << '\n'; |
je310 | 5:01e1e68309ae | 119 | // odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length()); |
je310 | 5:01e1e68309ae | 120 | // ss.clear(); |
je310 | 5:01e1e68309ae | 121 | // ss<< "w axis" << axNum << ".controller.config.vel_gain " << vel_gain << '\n'; |
je310 | 5:01e1e68309ae | 122 | // odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length()); |
je310 | 5:01e1e68309ae | 123 | // ss.clear(); |
je310 | 5:01e1e68309ae | 124 | // ss<< "w axis" << axNum << ".controller.config.pos_gain " << encoder_bandwidth << '\n'; |
je310 | 5:01e1e68309ae | 125 | // odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length()); |
je310 | 5:01e1e68309ae | 126 | // ss.clear(); |
je310 | 5:01e1e68309ae | 127 | // ss<< "w axis" << axNum << ".encoder.config.bandwidth " << pos_gain << '\n'; |
je310 | 5:01e1e68309ae | 128 | // odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length()); |
je310 | 5:01e1e68309ae | 129 | // ss.clear(); |
je310 | 5:01e1e68309ae | 130 | // ss<< "w axis" << axNum << ".controller.config.vel_integrator_gain " << vel_int << '\n'; |
je310 | 5:01e1e68309ae | 131 | // odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length()); |
je310 | 5:01e1e68309ae | 132 | |
je310 | 5:01e1e68309ae | 133 | } |
je310 | 5:01e1e68309ae | 134 | |
je310 | 4:778bc352c47f | 135 | void Axis::setMaxVel(float stepsPerSec) |
je310 | 4:778bc352c47f | 136 | { |
je310 | 4:778bc352c47f | 137 | std::stringstream ss; |
je310 | 4:778bc352c47f | 138 | ss<< "w axis" << axNum << ".controller.config.vel_limit " << stepsPerSec << '\n'; |
je310 | 4:778bc352c47f | 139 | buffered_pc.printf(ss.str().c_str()); |
je310 | 4:778bc352c47f | 140 | odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length()); |
je310 | 5:01e1e68309ae | 141 | } |
je310 | 5:01e1e68309ae | 142 | float Axis::readParam(string in) |
je310 | 5:01e1e68309ae | 143 | { |
je310 | 5:01e1e68309ae | 144 | Thread::wait(10); |
je310 | 5:01e1e68309ae | 145 | std::stringstream ss; |
je310 | 5:01e1e68309ae | 146 | ss <<"r axis" << axNum << in << '\n'; |
je310 | 5:01e1e68309ae | 147 | odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length()); |
je310 | 5:01e1e68309ae | 148 | return odrive->readFloat(); |
je310 | 5:01e1e68309ae | 149 | } |
je310 | 5:01e1e68309ae | 150 | float Axis::readBattery() |
je310 | 5:01e1e68309ae | 151 | { |
je310 | 5:01e1e68309ae | 152 | std::stringstream ss; |
je310 | 5:01e1e68309ae | 153 | ss <<"r vbus_voltage"<< '\n'; |
je310 | 5:01e1e68309ae | 154 | odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length()); |
je310 | 5:01e1e68309ae | 155 | return odrive->readFloat(); |
je310 | 5:01e1e68309ae | 156 | } |
je310 | 5:01e1e68309ae | 157 | |
je310 | 5:01e1e68309ae | 158 | |
je310 | 5:01e1e68309ae | 159 | float Axis::writeParam(string in, float val) |
je310 | 5:01e1e68309ae | 160 | { |
je310 | 5:01e1e68309ae | 161 | Thread::wait(10); |
je310 | 5:01e1e68309ae | 162 | std::stringstream ss; |
je310 | 5:01e1e68309ae | 163 | ss<< "w axis" << axNum << in<< " " << val << '\n'; |
je310 | 5:01e1e68309ae | 164 | odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length()); |
je310 | 5:01e1e68309ae | 165 | float read = readParam(in); |
je310 | 5:01e1e68309ae | 166 | if(read == val) return 0; |
je310 | 5:01e1e68309ae | 167 | else return 1; |
je310 | 5:01e1e68309ae | 168 | } |
je310 | 5:01e1e68309ae | 169 | |
je310 | 5:01e1e68309ae | 170 | int Axis::test() |
je310 | 5:01e1e68309ae | 171 | { |
je310 | 5:01e1e68309ae | 172 | int wait = 1; |
je310 | 5:01e1e68309ae | 173 | int waitTime = 10; |
je310 | 5:01e1e68309ae | 174 | if(wait) Thread::wait(waitTime); |
je310 | 5:01e1e68309ae | 175 | int error = 0; |
je310 | 5:01e1e68309ae | 176 | float battery = readBattery(); |
je310 | 5:01e1e68309ae | 177 | if(wait) Thread::wait(waitTime); |
je310 | 5:01e1e68309ae | 178 | battery = readBattery(); |
je310 | 5:01e1e68309ae | 179 | if(wait) Thread::wait(waitTime); |
je310 | 5:01e1e68309ae | 180 | //buffered_pc.printf("Battery: %f \r\n",battery); |
je310 | 5:01e1e68309ae | 181 | if (battery <1.0) error++; |
je310 | 5:01e1e68309ae | 182 | float param = readParam(".controller.config.vel_limit"); |
je310 | 5:01e1e68309ae | 183 | if(wait) Thread::wait(waitTime); |
je310 | 5:01e1e68309ae | 184 | //buffered_pc.printf("param: %f\r\n",param); |
je310 | 5:01e1e68309ae | 185 | writeParam(".controller.config.vel_limit", param+1.0); |
je310 | 5:01e1e68309ae | 186 | if(wait) Thread::wait(waitTime); |
je310 | 5:01e1e68309ae | 187 | float param2 = readParam(".controller.config.vel_limit"); |
je310 | 5:01e1e68309ae | 188 | if(wait) Thread::wait(waitTime); |
je310 | 5:01e1e68309ae | 189 | //buffered_pc.printf("param2: %f\r\n",param2); |
je310 | 5:01e1e68309ae | 190 | if(param2 != param + 1) error ++; |
je310 | 5:01e1e68309ae | 191 | writeParam(".controller.config.vel_limit", param); |
je310 | 5:01e1e68309ae | 192 | if(wait) Thread::wait(waitTime); |
je310 | 5:01e1e68309ae | 193 | float param3 = readParam(".controller.config.vel_limit"); |
je310 | 5:01e1e68309ae | 194 | if(wait) Thread::wait(waitTime); |
je310 | 5:01e1e68309ae | 195 | //buffered_pc.printf("param2: %f\r\n",param3); |
je310 | 5:01e1e68309ae | 196 | if(param3 != param) error ++; |
je310 | 5:01e1e68309ae | 197 | if (error!=0) { |
je310 | 5:01e1e68309ae | 198 | buffered_pc.printf("There have been %d errors on axis %d\r\n",error, name); |
je310 | 5:01e1e68309ae | 199 | } else { |
je310 | 5:01e1e68309ae | 200 | //buffered_pc.printf("there were no errors on axis %d :) \r\n", name); |
je310 | 5:01e1e68309ae | 201 | } |
je310 | 5:01e1e68309ae | 202 | return error; |
je310 | 5:01e1e68309ae | 203 | |
je310 | 3:10fa3102c2d7 | 204 | } |