joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

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?

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 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 }