Commit 4d65090f authored by Torbjørn Ludvigsen's avatar Torbjørn Ludvigsen 👷

Handle the way RepRapFirmware sends G95 and G96 over i2c

parent 697ac018
......@@ -40,30 +40,51 @@ void giveAngle() {
}
void handle_cmd(int numBytes) {
if(numBytes >= 1){
uint8_t cmd = Wire.read();
if(cmd == 0x5f){ // 95 in hexadecimal is 0x5f
if(numBytes == 5){ // Should receive a float upon G95
union {
uint8_t b[4]; // hard coded 4 instead of sizeof(float)
float fval;
} requested_torque;
int i = 0;
while(Wire.available()){
requested_torque.b[i] = Wire.read();
i++;
}
if(fabs(requested_torque.fval) < 0.01){
r = yw; // Set pos/vel setpoint to current position
mode = 'x';
} else if(fabs(requested_torque.fval) < 255.0) {
torque = requested_torque.fval;
mode = 't';
} // TODO: Don't know if positive torques are ever desired... Filter them out?
}
} else if(cmd == 0x60){ // 96 in hexadecimal is 0x60
yw_ref = yw;
bool got_G95_float = false;
bool got_G96 = false;
size_t bufsize = 50;
size_t offset = 0;
uint8_t recv[bufsize];
// Receive and store
size_t i = 0;
while(Wire.available() && i < bufsize){
recv[i] = Wire.read();
i++;
}
recv[i] = '\0';
// Parse
if(!strncmp((char *)recv, "G95 ", 4) && numBytes == 8){ // RepRapFirmware
got_G95_float = true;
offset = 4;
} else if((!strncmp((char *)recv, "G96", 3) && numBytes == 3) || (!strncmp((char *)recv, "G96 ", 4) && numBytes == 4)){ // RepRapFirmware
got_G96 = true;
} else if(recv[0] == 0x5f && numBytes == 5){ // Marlin
got_G95_float = true;
offset = 1;
} else if(recv[0] == 0x60){ // Marlin
got_G96 = true;
}
// Execute
if(got_G95_float){
union {
uint8_t b[4]; // hard coded 4 instead of sizeof(float)
float fval;
} requested_torque;
for(size_t i = 0; i < 4; i++){ // float has size 4
requested_torque.b[i] = recv[offset + i];
}
if(fabs(requested_torque.fval) < 0.01){
r = yw; // Set pos/vel setpoint to current position
mode = 'x';
} else if(fabs(requested_torque.fval) < 255.0) {
torque = requested_torque.fval;
mode = 't';
} // TODO: Don't know if positive torques are ever desired... Filter them out?
}else if(got_G96){
yw_ref = yw;
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment