Henry Herman / linact

Communicate with IAI Robotnet linear actuators

Dependents:   IAILinearActuators

linact.cpp@0:de88dc2515d3, 2014-02-24 (annotated)

Committer:
henryeherman
Date:
Mon Feb 24 05:54:34 2014 +0000
Revision:
0:de88dc2515d3
Initial commit of IAI code to mbed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
henryeherman 0:de88dc2515d3 1 #include <stdio.h>
henryeherman 0:de88dc2515d3 2 #include <string.h>
henryeherman 0:de88dc2515d3 3 #include <linact.hpp>
henryeherman 0:de88dc2515d3 4
henryeherman 0:de88dc2515d3 5 namespace IAI {
henryeherman 0:de88dc2515d3 6
henryeherman 0:de88dc2515d3 7
henryeherman 0:de88dc2515d3 8 LinearActuator::LinearActuator() {
henryeherman 0:de88dc2515d3 9 }
henryeherman 0:de88dc2515d3 10
henryeherman 0:de88dc2515d3 11 LinearActuator::~LinearActuator() {
henryeherman 0:de88dc2515d3 12 }
henryeherman 0:de88dc2515d3 13
henryeherman 0:de88dc2515d3 14 LinActBuf * LinearActuator::getGwStatusStr() {
henryeherman 0:de88dc2515d3 15 //gwstatus = "\x3f\x03\xf7\x00\x00\x02"
henryeherman 0:de88dc2515d3 16
henryeherman 0:de88dc2515d3 17 buffer.reset();
henryeherman 0:de88dc2515d3 18 buffer.readRegsisterStr(LINACT_GWSTATUS0, 2);
henryeherman 0:de88dc2515d3 19 return &buffer;
henryeherman 0:de88dc2515d3 20 }
henryeherman 0:de88dc2515d3 21
henryeherman 0:de88dc2515d3 22
henryeherman 0:de88dc2515d3 23 LinActBuf * LinearActuator::getGwStartStr() {
henryeherman 0:de88dc2515d3 24 //alwayson = "\x3f\x06\xf6\x00\x80\x00"
henryeherman 0:de88dc2515d3 25 // The 15-bit of the GWCTRL register must be set
henryeherman 0:de88dc2515d3 26 // to enable applicable control, page 163 from
henryeherman 0:de88dc2515d3 27 // ROBONET(ME0208-13A-A).pdf
henryeherman 0:de88dc2515d3 28 buffer.reset();
henryeherman 0:de88dc2515d3 29 buffer.writeRegisterStr(GWCTRL0, GWCTRL_APP_SIG);
henryeherman 0:de88dc2515d3 30 return &buffer;
henryeherman 0:de88dc2515d3 31 }
henryeherman 0:de88dc2515d3 32
henryeherman 0:de88dc2515d3 33
henryeherman 0:de88dc2515d3 34 unsigned short LinearActuator::getAxisReadAddress(unsigned int axisid) {
henryeherman 0:de88dc2515d3 35
henryeherman 0:de88dc2515d3 36 switch(axisid) {
henryeherman 0:de88dc2515d3 37 case 0:
henryeherman 0:de88dc2515d3 38 return AXIS0_BASE_RD;
henryeherman 0:de88dc2515d3 39 case 1:
henryeherman 0:de88dc2515d3 40 return AXIS1_BASE_RD;
henryeherman 0:de88dc2515d3 41 case 2:
henryeherman 0:de88dc2515d3 42 return AXIS2_BASE_RD;
henryeherman 0:de88dc2515d3 43 case 3:
henryeherman 0:de88dc2515d3 44 return AXIS3_BASE_RD;
henryeherman 0:de88dc2515d3 45 case 4:
henryeherman 0:de88dc2515d3 46 return AXIS4_BASE_RD;
henryeherman 0:de88dc2515d3 47 default:
henryeherman 0:de88dc2515d3 48 return 0;
henryeherman 0:de88dc2515d3 49 }
henryeherman 0:de88dc2515d3 50 }
henryeherman 0:de88dc2515d3 51
henryeherman 0:de88dc2515d3 52 unsigned short LinearActuator::getAxisWriteAddress(unsigned int axisid) {
henryeherman 0:de88dc2515d3 53
henryeherman 0:de88dc2515d3 54 switch(axisid) {
henryeherman 0:de88dc2515d3 55 case 0:
henryeherman 0:de88dc2515d3 56 return AXIS0_BASE_WR;
henryeherman 0:de88dc2515d3 57 case 1:
henryeherman 0:de88dc2515d3 58 return AXIS1_BASE_WR;
henryeherman 0:de88dc2515d3 59 case 2:
henryeherman 0:de88dc2515d3 60 return AXIS2_BASE_WR;
henryeherman 0:de88dc2515d3 61 case 3:
henryeherman 0:de88dc2515d3 62 return AXIS3_BASE_WR;
henryeherman 0:de88dc2515d3 63 case 4:
henryeherman 0:de88dc2515d3 64 return AXIS4_BASE_WR;
henryeherman 0:de88dc2515d3 65 default:
henryeherman 0:de88dc2515d3 66 return 0;
henryeherman 0:de88dc2515d3 67 }
henryeherman 0:de88dc2515d3 68 }
henryeherman 0:de88dc2515d3 69
henryeherman 0:de88dc2515d3 70 LinActBuf * LinearActuator::getAxisStatus(unsigned int axisid) {
henryeherman 0:de88dc2515d3 71 //axis1alarm = "\x3f\x03\xf7\x12\x00\x01"
henryeherman 0:de88dc2515d3 72 unsigned short axis_temp_reg;
henryeherman 0:de88dc2515d3 73 buffer.reset();
henryeherman 0:de88dc2515d3 74 axis_temp_reg = getAxisReadAddress(axisid) + STATUS_SIG_OFFSET;
henryeherman 0:de88dc2515d3 75 buffer.readRegsisterStr(axis_temp_reg, 1);
henryeherman 0:de88dc2515d3 76 return &buffer;
henryeherman 0:de88dc2515d3 77 }
henryeherman 0:de88dc2515d3 78
henryeherman 0:de88dc2515d3 79 LinActBuf * LinearActuator::getAxisPos(unsigned int axisid) {
henryeherman 0:de88dc2515d3 80 unsigned short axis_temp_reg;
henryeherman 0:de88dc2515d3 81 buffer.reset();
henryeherman 0:de88dc2515d3 82 axis_temp_reg = getAxisReadAddress(axisid) + POS_SET_LO_OFFSET;
henryeherman 0:de88dc2515d3 83 buffer.readRegsisterStr(axis_temp_reg, 2); // Read 2 to get lo and hi
henryeherman 0:de88dc2515d3 84 return &buffer;
henryeherman 0:de88dc2515d3 85 }
henryeherman 0:de88dc2515d3 86
henryeherman 0:de88dc2515d3 87 LinActBuf * LinearActuator::getSetAxisPos(unsigned int axisid, unsigned int position) {
henryeherman 0:de88dc2515d3 88 unsigned short axis_temp_reg;
henryeherman 0:de88dc2515d3 89 LinActBuf temp;
henryeherman 0:de88dc2515d3 90 buffer.reset();
henryeherman 0:de88dc2515d3 91 axis_temp_reg = getAxisWriteAddress(axisid) + POS_SET_LO_OFFSET;
henryeherman 0:de88dc2515d3 92 temp.buf[2] = (unsigned char)((position & 0xFF000000) >> 24);
henryeherman 0:de88dc2515d3 93 temp.buf[3] = (unsigned char)((position & 0x00FF0000) >> 16);
henryeherman 0:de88dc2515d3 94 temp.buf[0] = (unsigned char)((position & 0x0000FF00) >> 8);
henryeherman 0:de88dc2515d3 95 temp.buf[1] = (unsigned char)((position & 0x000000FF));
henryeherman 0:de88dc2515d3 96 temp.len = 4;
henryeherman 0:de88dc2515d3 97 buffer.writeMultiRegisterStr(axis_temp_reg, 2, temp.buf, temp.len);
henryeherman 0:de88dc2515d3 98 return &buffer;
henryeherman 0:de88dc2515d3 99 }
henryeherman 0:de88dc2515d3 100
henryeherman 0:de88dc2515d3 101
henryeherman 0:de88dc2515d3 102 LinActBuf * LinearActuator::getAxisStart(unsigned int axisid) {
henryeherman 0:de88dc2515d3 103 //startcmd0 = "\x3f\x06\xf6\x0b\x00\x11"
henryeherman 0:de88dc2515d3 104 unsigned short int axisreg;
henryeherman 0:de88dc2515d3 105 unsigned short value;
henryeherman 0:de88dc2515d3 106 buffer.reset();
henryeherman 0:de88dc2515d3 107 axisreg = getAxisWriteAddress(axisid) + CNTRL_SIG_OFFSET;
henryeherman 0:de88dc2515d3 108 value = (AXIS_CTRL_SON|AXIS_CTRL_CSTR);
henryeherman 0:de88dc2515d3 109 buffer.writeRegisterStr(axisreg, value);
henryeherman 0:de88dc2515d3 110 return &buffer;
henryeherman 0:de88dc2515d3 111 }
henryeherman 0:de88dc2515d3 112
henryeherman 0:de88dc2515d3 113 LinActBuf * LinearActuator::getAxisPause(unsigned int axisid) {
henryeherman 0:de88dc2515d3 114 //pausecmd = "\x3f\x06\xf6\x0b\x00\x14"
henryeherman 0:de88dc2515d3 115 unsigned short int axisreg;
henryeherman 0:de88dc2515d3 116 unsigned short value;
henryeherman 0:de88dc2515d3 117 buffer.reset();
henryeherman 0:de88dc2515d3 118 axisreg = getAxisWriteAddress(axisid) + CNTRL_SIG_OFFSET;
henryeherman 0:de88dc2515d3 119 value = (AXIS_CTRL_SON|AXIS_CTRL_STP);
henryeherman 0:de88dc2515d3 120 buffer.writeRegisterStr(axisreg, value);
henryeherman 0:de88dc2515d3 121 return &buffer;
henryeherman 0:de88dc2515d3 122 }
henryeherman 0:de88dc2515d3 123
henryeherman 0:de88dc2515d3 124 LinActBuf * LinearActuator::getAxisReset(unsigned int axisid) {
henryeherman 0:de88dc2515d3 125 // resetcmd0 = "\x3f\x06\xf6\x0b\x00\x08"
henryeherman 0:de88dc2515d3 126 unsigned short int axisreg;
henryeherman 0:de88dc2515d3 127 unsigned short value;
henryeherman 0:de88dc2515d3 128 buffer.reset();
henryeherman 0:de88dc2515d3 129 axisreg = getAxisWriteAddress(axisid) + CNTRL_SIG_OFFSET;
henryeherman 0:de88dc2515d3 130 value = (AXIS_CTRL_RES);
henryeherman 0:de88dc2515d3 131 buffer.writeRegisterStr(axisreg, value);
henryeherman 0:de88dc2515d3 132 return &buffer;
henryeherman 0:de88dc2515d3 133 }
henryeherman 0:de88dc2515d3 134
henryeherman 0:de88dc2515d3 135 LinActBuf * LinearActuator::getAxisHome(unsigned int axisid) {
henryeherman 0:de88dc2515d3 136 // resetcmd0 = "\x3f\x06\xf6\x0b\x00\x08"
henryeherman 0:de88dc2515d3 137 unsigned short int axisreg;
henryeherman 0:de88dc2515d3 138 unsigned short value;
henryeherman 0:de88dc2515d3 139 buffer.reset();
henryeherman 0:de88dc2515d3 140 axisreg = getAxisWriteAddress(axisid) + CNTRL_SIG_OFFSET;
henryeherman 0:de88dc2515d3 141 value = (AXIS_CTRL_HOME|AXIS_CTRL_SON);
henryeherman 0:de88dc2515d3 142 buffer.writeRegisterStr(axisreg, value);
henryeherman 0:de88dc2515d3 143 return &buffer;
henryeherman 0:de88dc2515d3 144 }
henryeherman 0:de88dc2515d3 145
henryeherman 0:de88dc2515d3 146 LinActBuf * LinearActuator::getAxisOn(unsigned int axisid) {
henryeherman 0:de88dc2515d3 147 // resetcmd0 = "\x3f\x06\xf6\x0b\x00\x08"
henryeherman 0:de88dc2515d3 148 unsigned short int axisreg;
henryeherman 0:de88dc2515d3 149 unsigned short value;
henryeherman 0:de88dc2515d3 150 buffer.reset();
henryeherman 0:de88dc2515d3 151 axisreg = getAxisWriteAddress(axisid) + CNTRL_SIG_OFFSET;
henryeherman 0:de88dc2515d3 152 value = (AXIS_CTRL_SON);
henryeherman 0:de88dc2515d3 153 buffer.writeRegisterStr(axisreg, value);
henryeherman 0:de88dc2515d3 154 return &buffer;
henryeherman 0:de88dc2515d3 155 }
henryeherman 0:de88dc2515d3 156
henryeherman 0:de88dc2515d3 157 LinActBuf * LinearActuator::getAxisBrakeRelease(unsigned int axisid) {
henryeherman 0:de88dc2515d3 158 unsigned short int axisreg;
henryeherman 0:de88dc2515d3 159 unsigned short value;
henryeherman 0:de88dc2515d3 160 buffer.reset();
henryeherman 0:de88dc2515d3 161 axisreg = getAxisWriteAddress(axisid) + CNTRL_SIG_OFFSET;
henryeherman 0:de88dc2515d3 162 value = (AXIS_CTRL_BKRL);
henryeherman 0:de88dc2515d3 163 buffer.writeRegisterStr(axisreg, value);
henryeherman 0:de88dc2515d3 164 return &buffer;
henryeherman 0:de88dc2515d3 165 }
henryeherman 0:de88dc2515d3 166
henryeherman 0:de88dc2515d3 167 float LinearActuator::getPosition() {
henryeherman 0:de88dc2515d3 168 long current_pos;
henryeherman 0:de88dc2515d3 169 unsigned char *payload;
henryeherman 0:de88dc2515d3 170
henryeherman 0:de88dc2515d3 171 if(buffer.rxdata()==0 || checkChecksum() < 0)
henryeherman 0:de88dc2515d3 172 return NOTPOSMSGERR;
henryeherman 0:de88dc2515d3 173
henryeherman 0:de88dc2515d3 174 payload = buffer.rxdata();
henryeherman 0:de88dc2515d3 175 buffer.rxdatalen();
henryeherman 0:de88dc2515d3 176
henryeherman 0:de88dc2515d3 177 if(buffer.rxdatalen()!=4)
henryeherman 0:de88dc2515d3 178 return NOTPOSMSGERR;
henryeherman 0:de88dc2515d3 179
henryeherman 0:de88dc2515d3 180 current_pos = (payload[1]<<0)+(payload[0]<<8) +
henryeherman 0:de88dc2515d3 181 ((payload[2] & 0x7F)<<24)+(payload[3] <<16);
henryeherman 0:de88dc2515d3 182
henryeherman 0:de88dc2515d3 183 if(0x80 & payload[2]) {
henryeherman 0:de88dc2515d3 184 // Check if we need to flip our sign
henryeherman 0:de88dc2515d3 185 //printf("Negative\r\n");
henryeherman 0:de88dc2515d3 186 current_pos = current_pos * -1;
henryeherman 0:de88dc2515d3 187 }
henryeherman 0:de88dc2515d3 188
henryeherman 0:de88dc2515d3 189 return (float) (current_pos / 100.0);
henryeherman 0:de88dc2515d3 190
henryeherman 0:de88dc2515d3 191 }
henryeherman 0:de88dc2515d3 192
henryeherman 0:de88dc2515d3 193 LinActBuf * LinearActuator::getBuffer() {
henryeherman 0:de88dc2515d3 194 return &buffer;
henryeherman 0:de88dc2515d3 195 }
henryeherman 0:de88dc2515d3 196
henryeherman 0:de88dc2515d3 197 void LinearActuator::send() {
henryeherman 0:de88dc2515d3 198 // Drive WR pin
henryeherman 0:de88dc2515d3 199 for(int i=0;i<buffer.len;i++){
henryeherman 0:de88dc2515d3 200 putchar(buffer.buf[i]);
henryeherman 0:de88dc2515d3 201 }
henryeherman 0:de88dc2515d3 202 // Stop driving WR pin to receive
henryeherman 0:de88dc2515d3 203 }
henryeherman 0:de88dc2515d3 204
henryeherman 0:de88dc2515d3 205 LinActBuf * LinearActuator::receiveStdin(int len) {
henryeherman 0:de88dc2515d3 206 // Fill buffer
henryeherman 0:de88dc2515d3 207 for(int i=0;i<len;i++) {
henryeherman 0:de88dc2515d3 208 pushByteRxBuffer(getc(stdin));
henryeherman 0:de88dc2515d3 209 //printf("Inbuf.len=%d", rxbuf.len);
henryeherman 0:de88dc2515d3 210 }
henryeherman 0:de88dc2515d3 211 LINACTINFO("Receive: %s\r\n", buffer.rx_as_string());
henryeherman 0:de88dc2515d3 212 return &buffer;
henryeherman 0:de88dc2515d3 213 }
henryeherman 0:de88dc2515d3 214
henryeherman 0:de88dc2515d3 215 LinActBuf * LinearActuator::pushByteRxBuffer(char c) {
henryeherman 0:de88dc2515d3 216 buffer.pushRx(c);
henryeherman 0:de88dc2515d3 217 return &buffer;
henryeherman 0:de88dc2515d3 218 }
henryeherman 0:de88dc2515d3 219
henryeherman 0:de88dc2515d3 220 int LinearActuator::checkChecksum() {
henryeherman 0:de88dc2515d3 221 return buffer.rxvalidate();
henryeherman 0:de88dc2515d3 222 }
henryeherman 0:de88dc2515d3 223
henryeherman 0:de88dc2515d3 224
henryeherman 0:de88dc2515d3 225 } // End Namespace IAI