From 88b4f945ebdbca8a0a6eb709eab4e4cb3459a046 Mon Sep 17 00:00:00 2001 From: fre-ber Date: Wed, 5 Sep 2018 21:07:41 +0200 Subject: [PATCH 1/3] Early bail instead of nested "if" makes it a bit easier to trace program flow, I think. --- Part8/Slave01/Slave01.ino | 663 +++++++++++++++++++------------------- 1 file changed, 331 insertions(+), 332 deletions(-) diff --git a/Part8/Slave01/Slave01.ino b/Part8/Slave01/Slave01.ino index 917aade..b9a905c 100644 --- a/Part8/Slave01/Slave01.ino +++ b/Part8/Slave01/Slave01.ino @@ -147,345 +147,344 @@ void setup() { void loop() { unsigned long currentMillis = millis(); - if (currentMillis - previousMillis >= interval) { //start timed event - previousMillis = currentMillis; - - but1 = digitalRead(25); - but2 = digitalRead(27); - but3 = digitalRead(29); - but4 = digitalRead(31); - - home1 = digitalRead(43); - home2 = digitalRead(45); - home3 = digitalRead(47); - home4 = digitalRead(49); - home5 = digitalRead(51); - home6 = digitalRead(53); - - - // *****************************right leg********************************** - - if (but4 == 0 && flag == 0) { - digitalWrite(39, LOW); - - Serial.println("Right Motor 0"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(0, requested_state, false); // don't wait - - while (home4a == 1) { - home4 = digitalRead(49); - if (home4 == 1) { - previousFilterMillis = millis(); - } - else if (home4 == 0 && millis() - previousFilterMillis > filterTime) { - home4a = 0; - } - // move motor 0 - odrive2.SetVelocity(0, 10000); - } - // stop motor 0 - odrive2.SetVelocity(0, 0); - delay(300); - //save zero position and back off two revolutions - Serial3 << "r axis" << 0 << ".encoder.pos_estimate\n"; - home4Offset = odrive2.readInt(); - Serial.println(home4Offset); - odrive2.SetPosition(0, (home4Offset-(8192*2))); // back off two revolutions - delay (500); // wait for that to properly finish - - Serial.println("Right Motor 1"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(1, requested_state, false); // don't wait - - while (home3a == 1) { - home3 = digitalRead(47); - if (home3 == 1) { - previousFilterMillis = millis(); - } - else if (home3 == 0 && millis() - previousFilterMillis > filterTime) { - home3a = 0; - } - // move motor 1 - odrive2.SetVelocity(1, 10000); - } - //stop motor 1 - odrive2.SetVelocity(1, 0); - delay(300); - //save zero position and back off two revolutions - Serial3 << "r axis" << 1 << ".encoder.pos_estimate\n"; - home3Offset = odrive2.readInt(); - Serial.println(home3Offset); - odrive2.SetPosition(1, (home3Offset-(8192*2))); // back off one revolution - flag = 1; - digitalWrite(37, HIGH); // Yellow top left - } - + if (currentMillis - previousMillis < interval) { + return; + } + //start timed event + previousMillis = currentMillis; + + but1 = digitalRead(25); + but2 = digitalRead(27); + but3 = digitalRead(29); + but4 = digitalRead(31); + home1 = digitalRead(43); + home2 = digitalRead(45); + home3 = digitalRead(47); + home4 = digitalRead(49); + home5 = digitalRead(51); + home6 = digitalRead(53); + + + // *****************************right leg********************************** + + if (but4 == 0 && flag == 0) { + digitalWrite(39, LOW); + + Serial.println("Right Motor 0"); + requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; + Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; + odrive2.run_state(0, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; + Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; + odrive2.run_state(0, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; + Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; + odrive2.run_state(0, requested_state, false); // don't wait + + while (home4a == 1) { + home4 = digitalRead(49); + if (home4 == 1) { + previousFilterMillis = millis(); + } + else if (home4 == 0 && millis() - previousFilterMillis > filterTime) { + home4a = 0; + } + // move motor 0 + odrive2.SetVelocity(0, 10000); + } + // stop motor 0 + odrive2.SetVelocity(0, 0); + delay(300); + //save zero position and back off two revolutions + Serial3 << "r axis" << 0 << ".encoder.pos_estimate\n"; + home4Offset = odrive2.readInt(); + Serial.println(home4Offset); + odrive2.SetPosition(0, (home4Offset-(8192*2))); // back off two revolutions + delay (500); // wait for that to properly finish + + Serial.println("Right Motor 1"); + requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; + Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; + odrive2.run_state(1, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; + Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; + odrive2.run_state(1, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; + Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; + odrive2.run_state(1, requested_state, false); // don't wait + + while (home3a == 1) { + home3 = digitalRead(47); + if (home3 == 1) { + previousFilterMillis = millis(); + } + else if (home3 == 0 && millis() - previousFilterMillis > filterTime) { + home3a = 0; + } + // move motor 1 + odrive2.SetVelocity(1, 10000); + } + //stop motor 1 + odrive2.SetVelocity(1, 0); + delay(300); + //save zero position and back off two revolutions + Serial3 << "r axis" << 1 << ".encoder.pos_estimate\n"; + home3Offset = odrive2.readInt(); + Serial.println(home3Offset); + odrive2.SetPosition(1, (home3Offset-(8192*2))); // back off one revolution + flag = 1; + digitalWrite(37, HIGH); // Yellow top left + } + + + + // *********************************left leg************************************ + + else if (but3 == 0 && flag == 1) { + digitalWrite(37, LOW); + + Serial.println("Left Motor 0"); + requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; + Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; + odrive1.run_state(0, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; + Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; + odrive1.run_state(0, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; + Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; + odrive1.run_state(0, requested_state, false); // don't wait + + while (home2a == 1) { + home2 = digitalRead(45); + if (home2 == 1) { + previousFilterMillis = millis(); + } + else if (home2 == 0 && millis() - previousFilterMillis > filterTime) { + home2a = 0; + } + // move motor 0 + odrive1.SetVelocity(0, 10000); + } + // stop motor 0 + odrive1.SetVelocity(0, 0); + delay(300); + //save zero position and back off two revolutions + Serial2 << "r axis" << 0 << ".encoder.pos_estimate\n"; + home2Offset = odrive1.readInt(); + Serial.println(home2Offset); + odrive1.SetPosition(0, (home2Offset-(8192*2))); // back off two revolutions + delay (500); // wait for that to properly finish + + Serial.println("Left Motor 1"); + requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; + Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; + odrive1.run_state(1, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; + Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; + odrive1.run_state(1, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; + Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; + odrive1.run_state(1, requested_state, false); // don't wait + + while (home1a == 1) { + home1 = digitalRead(43); + if (home1 == 1) { + previousFilterMillis = millis(); + } + else if (home1 == 0 && millis() - previousFilterMillis > filterTime) { + home1a = 0; + } + // move motor 1 + odrive1.SetVelocity(1, 10000); + } + //stop motor 1 + odrive1.SetVelocity(1, 0); + delay(300); + //save zero position and back off two revolutions + Serial2 << "r axis" << 1 << ".encoder.pos_estimate\n"; + home1Offset = odrive1.readInt(); + Serial.println(home1Offset); + odrive1.SetPosition(1, (home1Offset-(8192*2))); // back off one revolution + + // time to boost the legs up + digitalWrite(37, HIGH); // yellow left + digitalWrite(39, HIGH); // yellow right + flag = 2; + } + + else if (but4 == 0 || but3 == 0 && flag == 2) { + digitalWrite(37, LOW); + digitalWrite(39, LOW); + + for (int axis = 0; axis < 2; ++axis) { + Serial2 << "w axis" << axis << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast ODrive1 + } + for (int axis = 0; axis < 2; ++axis) { + Serial3 << "w axis" << axis << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast ODrive2 + } + + odrive2.SetPosition(0, (home4Offset - 163840)); // move legs straight right + odrive2.SetPosition(1, (home3Offset - 245760)); // move legs straight right + + odrive1.SetPosition(0, (home2Offset - 163840)); // move legs straight left + odrive1.SetPosition(1, (home1Offset - 245760)); // move legs straight left + + digitalWrite(35, HIGH); // white LED right - ready for undercarriage calibration + flag = 3; + } + + + // *****************************leg motor undercarriage************************************** + + else if (but2 == 0 && flag == 3) { + // bend right leg + digitalWrite(35, LOW); + odrive2.SetPosition(0, (home4Offset - 16384)); // move leg bent right + odrive2.SetPosition(1, (home3Offset - 16384)); // move leg bent right + delay (2000); // wait for leg to bend + + // calibrate right leg undercarriage, ODrive axis 1 + Serial.println("Right Leg undercarriage"); + requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; + Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; + odrive0.run_state(1, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; + Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; + odrive0.run_state(1, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; + Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; + odrive0.run_state(1, requested_state, false); // don't wait + delay (500); // wait for that to properly finish + + // move undercarriage ODrive axis 1 under it hits the home switch 6 + + while (home6a == 1) { + home6 = digitalRead(53); + if (home6 == 1) { + previousFilterMillis = millis(); + } + else if (home6 == 0 && millis() - previousFilterMillis > filterTime) { + home6a = 0; + } + // move motor 1 + odrive0.SetVelocity(1, -10000); + } + //stop motor 1 + odrive0.SetVelocity(1, 0); + + //save zero position + odrive_serial << "r axis" << 1 << ".encoder.pos_estimate\n"; + home6Offset = odrive0.readInt(); + Serial.print(home6Offset); + Serial.print(" , "); + while ((home6Offset < -122000) || (home6Offset > -52000)) { // filter because software serial is a bit rubbish + odrive_serial << "r axis" << 1 << ".encoder.pos_estimate\n"; + home6Offset = odrive0.readInt(); + } + Serial.println(home6Offset); // print filtered value + + odrive_serial << "w axis" << 1 << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast for Odrive 0 / axis 1 + odrive0.SetPosition(1, (home6Offset+87654)); // back off 25mm + delay(3000); // wait for leg to move out again + + // put leg straight again + odrive2.SetPosition(0, (home4Offset - 163840)); // move legs straight right + odrive2.SetPosition(1, (home3Offset - 245760)); // move legs straight right + + digitalWrite(33, HIGH); // white LED left bottom - ready for the other side + flag = 4; + } + + //**************** do the other leg ***************** + + else if (but1 == 0 && flag == 4) { + digitalWrite(33, LOW); + odrive1.SetPosition(0, (home2Offset - 16384)); // move leg bent left + odrive1.SetPosition(1, (home1Offset - 16384)); // move leg bent left + delay (2000); // wait for leg to bend + + // calibrate right leg undercarriage, ODrive axis 0 + Serial.println("Right Leg undercarriage"); + requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; + Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; + odrive0.run_state(0, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; + Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; + odrive0.run_state(0, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; + Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; + odrive0.run_state(0, requested_state, false); // don't wait + delay (500); // wait for that to properly finish + + // move undercarriage ODrive axis 0 under it hits the home switch 5 + + while (home5a == 1) { + home5 = digitalRead(51); + if (home5 == 1) { + previousFilterMillis = millis(); + } + else if (home5 == 0 && millis() - previousFilterMillis > filterTime) { + home5a = 0; + } + // move motor 0 + odrive0.SetVelocity(0, -10000); + } - // *********************************left leg************************************ - - else if (but3 == 0 && flag == 1) { - digitalWrite(37, LOW); - - Serial.println("Left Motor 0"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(0, requested_state, false); // don't wait - - while (home2a == 1) { - home2 = digitalRead(45); - if (home2 == 1) { - previousFilterMillis = millis(); - } - else if (home2 == 0 && millis() - previousFilterMillis > filterTime) { - home2a = 0; - } - // move motor 0 - odrive1.SetVelocity(0, 10000); - } - // stop motor 0 - odrive1.SetVelocity(0, 0); - delay(300); - //save zero position and back off two revolutions - Serial2 << "r axis" << 0 << ".encoder.pos_estimate\n"; - home2Offset = odrive1.readInt(); - Serial.println(home2Offset); - odrive1.SetPosition(0, (home2Offset-(8192*2))); // back off two revolutions - delay (500); // wait for that to properly finish - - Serial.println("Left Motor 1"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(1, requested_state, false); // don't wait - - while (home1a == 1) { - home1 = digitalRead(43); - if (home1 == 1) { - previousFilterMillis = millis(); - } - else if (home1 == 0 && millis() - previousFilterMillis > filterTime) { - home1a = 0; - } - // move motor 1 - odrive1.SetVelocity(1, 10000); - } - //stop motor 1 - odrive1.SetVelocity(1, 0); - delay(300); - //save zero position and back off two revolutions - Serial2 << "r axis" << 1 << ".encoder.pos_estimate\n"; - home1Offset = odrive1.readInt(); - Serial.println(home1Offset); - odrive1.SetPosition(1, (home1Offset-(8192*2))); // back off one revolution - - // time to boost the legs up - digitalWrite(37, HIGH); // yellow left - digitalWrite(39, HIGH); // yellow right - flag = 2; - } - - else if (but4 == 0 || but3 == 0 && flag == 2) { - digitalWrite(37, LOW); - digitalWrite(39, LOW); - - for (int axis = 0; axis < 2; ++axis) { - Serial2 << "w axis" << axis << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast ODrive1 - } - for (int axis = 0; axis < 2; ++axis) { - Serial3 << "w axis" << axis << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast ODrive2 - } - - odrive2.SetPosition(0, (home4Offset - 163840)); // move legs straight right - odrive2.SetPosition(1, (home3Offset - 245760)); // move legs straight right - - odrive1.SetPosition(0, (home2Offset - 163840)); // move legs straight left - odrive1.SetPosition(1, (home1Offset - 245760)); // move legs straight left - - digitalWrite(35, HIGH); // white LED right - ready for undercarriage calibration - flag = 3; - } - - - // *****************************leg motor undercarriage************************************** - - else if (but2 == 0 && flag == 3) { - // bend right leg - digitalWrite(35, LOW); - odrive2.SetPosition(0, (home4Offset - 16384)); // move leg bent right - odrive2.SetPosition(1, (home3Offset - 16384)); // move leg bent right - delay (2000); // wait for leg to bend - - // calibrate right leg undercarriage, ODrive axis 1 - Serial.println("Right Leg undercarriage"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(1, requested_state, false); // don't wait - delay (500); // wait for that to properly finish - - // move undercarriage ODrive axis 1 under it hits the home switch 6 - - while (home6a == 1) { - home6 = digitalRead(53); - if (home6 == 1) { - previousFilterMillis = millis(); - } - else if (home6 == 0 && millis() - previousFilterMillis > filterTime) { - home6a = 0; - } - // move motor 1 - odrive0.SetVelocity(1, -10000); - } - //stop motor 1 - odrive0.SetVelocity(1, 0); - - //save zero position - odrive_serial << "r axis" << 1 << ".encoder.pos_estimate\n"; - home6Offset = odrive0.readInt(); - Serial.print(home6Offset); - Serial.print(" , "); - while ((home6Offset < -122000) || (home6Offset > -52000)) { // filter because software serial is a bit rubbish - odrive_serial << "r axis" << 1 << ".encoder.pos_estimate\n"; - home6Offset = odrive0.readInt(); - } - Serial.println(home6Offset); // print filtered value - - odrive_serial << "w axis" << 1 << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast for Odrive 0 / axis 1 - odrive0.SetPosition(1, (home6Offset+87654)); // back off 25mm - delay(3000); // wait for leg to move out again - - // put leg straight again - odrive2.SetPosition(0, (home4Offset - 163840)); // move legs straight right - odrive2.SetPosition(1, (home3Offset - 245760)); // move legs straight right - - digitalWrite(33, HIGH); // white LED left bottom - ready for the other side - flag = 4; - } - - //**************** do the other leg ***************** - - else if (but1 == 0 && flag == 4) { - digitalWrite(33, LOW); - odrive1.SetPosition(0, (home2Offset - 16384)); // move leg bent left - odrive1.SetPosition(1, (home1Offset - 16384)); // move leg bent left - delay (2000); // wait for leg to bend - - // calibrate right leg undercarriage, ODrive axis 0 - Serial.println("Right Leg undercarriage"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(0, requested_state, false); // don't wait - delay (500); // wait for that to properly finish - - // move undercarriage ODrive axis 0 under it hits the home switch 5 - - while (home5a == 1) { - home5 = digitalRead(51); - if (home5 == 1) { - previousFilterMillis = millis(); - } - else if (home5 == 0 && millis() - previousFilterMillis > filterTime) { - home5a = 0; - } - // move motor 0 - odrive0.SetVelocity(0, -10000); - } + //stop motor 0 + odrive0.SetVelocity(0, 0); + + //save zero position + odrive_serial << "r axis" << 0 << ".encoder.pos_estimate\n"; + home5Offset = odrive0.readInt(); + Serial.print(home5Offset); + Serial.print(" , "); + while ((home5Offset < -122000) || (home5Offset > -52000)) { // filter because software serial is a bit rubbish + odrive_serial << "r axis" << 0 << ".encoder.pos_estimate\n"; + home5Offset = odrive0.readInt(); + } + Serial.println(home5Offset); // print filtered value + + odrive_serial << "w axis" << 0 << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast for Odrive 0 / axis 0 + odrive0.SetPosition(0, (home6Offset+87654)); // back off 25mm + delay(3000); // wait for leg to move out again + + // put leg straight again + odrive1.SetPosition(0, (home2Offset - 163840)); // move legs straight left + odrive1.SetPosition(1, (home1Offset - 245760)); // move legs straight left + + delay(3000); // wait for the leg to be straight + flag = 5; // proceed to next stage + } + + if (flag == 5) { // main output to leg starts here + + // ***********serial receive from master*************** + + ET3.receiveData(); - //stop motor 0 - odrive0.SetVelocity(0, 0); - - //save zero position - odrive_serial << "r axis" << 0 << ".encoder.pos_estimate\n"; - home5Offset = odrive0.readInt(); - Serial.print(home5Offset); - Serial.print(" , "); - while ((home5Offset < -122000) || (home5Offset > -52000)) { // filter because software serial is a bit rubbish - odrive_serial << "r axis" << 0 << ".encoder.pos_estimate\n"; - home5Offset = odrive0.readInt(); - } - Serial.println(home5Offset); // print filtered value - - odrive_serial << "w axis" << 0 << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast for Odrive 0 / axis 0 - odrive0.SetPosition(0, (home6Offset+87654)); // back off 25mm - delay(3000); // wait for leg to move out again - - // put leg straight again - odrive1.SetPosition(0, (home2Offset - 163840)); // move legs straight left - odrive1.SetPosition(1, (home1Offset - 245760)); // move legs straight left - - delay(3000); // wait for the leg to be straight - flag = 5; // proceed to next stage - } - - if (flag == 5) { // main output to leg starts here - - // ***********serial receive from master*************** + Serial.print(mydata_back.shoulderR); // print original test value + Serial.print(" , "); + shoulderRFiltered = filter(mydata_back.shoulderR, shoulderRFiltered); + Serial.print(shoulderRFiltered); - ET3.receiveData(); - - Serial.print(mydata_back.shoulderR); // print original test value - Serial.print(" , "); - shoulderRFiltered = filter(mydata_back.shoulderR, shoulderRFiltered); - Serial.print(shoulderRFiltered); + Serial.print(" , "); - Serial.print(" , "); + Serial.print(mydata_back.shoulderL); // print original test value + Serial.print(" , "); + shoulderLFiltered = filter(mydata_back.shoulderL, shoulderLFiltered); + Serial.println(shoulderLFiltered); - Serial.print(mydata_back.shoulderL); // print original test value - Serial.print(" , "); - shoulderLFiltered = filter(mydata_back.shoulderL, shoulderLFiltered); - Serial.println(shoulderLFiltered); - - shoulderRFiltered2 = shoulderRFiltered*3490; // work out encoder counts per milimeter - shoulderLFiltered2 = shoulderLFiltered*3490; // work out encoder counts per milimeter - - home2Home = home2Offset - 163840; // the current position it's at - home4Home = home4Offset - 163840; // the current position it's at - - odrive2.SetPosition(0, (home4Home + shoulderRFiltered2)); // use the test remote data to move the actuator - odrive1.SetPosition(0, (home2Home + shoulderLFiltered2)); // use the test remote data to move the actuator - - } // end of main output to leg - - } // end of timed event + shoulderRFiltered2 = shoulderRFiltered*3490; // work out encoder counts per milimeter + shoulderLFiltered2 = shoulderLFiltered*3490; // work out encoder counts per milimeter - + home2Home = home2Offset - 163840; // the current position it's at + home4Home = home4Offset - 163840; // the current position it's at + + odrive2.SetPosition(0, (home4Home + shoulderRFiltered2)); // use the test remote data to move the actuator + odrive1.SetPosition(0, (home2Home + shoulderLFiltered2)); // use the test remote data to move the actuator + + } // end of main output to leg } From 05854220e7c9923fd9b6fdfa31efea505d260bf9 Mon Sep 17 00:00:00 2001 From: fre-ber Date: Wed, 5 Sep 2018 21:36:41 +0200 Subject: [PATCH 2/3] Use #define instead of "magic numbers" for LED and switch pins. Easier to understand the code as well as easier to modify for the other set of legs in Slave02. --- Part8/Slave01/Slave01.ino | 103 ++++++++++++++++++++++---------------- 1 file changed, 60 insertions(+), 43 deletions(-) diff --git a/Part8/Slave01/Slave01.ino b/Part8/Slave01/Slave01.ino index b9a905c..637073e 100644 --- a/Part8/Slave01/Slave01.ino +++ b/Part8/Slave01/Slave01.ino @@ -2,6 +2,24 @@ #include #include +#define LED_LEFT_BOTTOM_WHITE 33 +#define LED_RIGHT_BOTTOM_WHITE 35 +#define LED_LEFT_TOP_YELLOW 37 +#define LED_RIGHT_TOP_YELLOW 39 + +#define BUTTON1_PIN 25 +#define BUTTON2_PIN 27 +#define BUTTON3_PIN 29 +#define BUTTON4_PIN 31 + +#define HOME1_PIN 43 +#define HOME2_PIN 45 +#define HOME3_PIN 47 +#define HOME4_PIN 49 +#define HOME5_PIN 51 +#define HOME6_PIN 53 + + //**************Slave Arduinos**************** EasyTransfer ET3; // slave 1 - back of robot @@ -88,26 +106,26 @@ void setup() { // LEDs - pinMode(33, OUTPUT); // white left bottom - pinMode(35, OUTPUT); // white right bottom - pinMode(37, OUTPUT); // yellow left top - pinMode(39, OUTPUT); // yellow right top + pinMode(LED_LEFT_BOTTOM_WHITE, OUTPUT); // white left bottom + pinMode(LED_RIGHT_BOTTOM_WHITE, OUTPUT); // white right bottom + pinMode(LED_LEFT_TOP_YELLOW, OUTPUT); // yellow left top + pinMode(LED_RIGHT_TOP_YELLOW, OUTPUT); // yellow right top // control panel buttons - pinMode(25, INPUT_PULLUP); // left bottom - pinMode(27, INPUT_PULLUP); // right bottom - pinMode(29, INPUT_PULLUP); // left top - pinMode(31, INPUT_PULLUP); // right top + pinMode(BUTTON1_PIN, INPUT_PULLUP); // left bottom + pinMode(BUTTON2_PIN, INPUT_PULLUP); // right bottom + pinMode(BUTTON3_PIN, INPUT_PULLUP); // left top + pinMode(BUTTON4_PIN, INPUT_PULLUP); // right top // home switches - pinMode(43, INPUT_PULLUP); - pinMode(45, INPUT_PULLUP); - pinMode(47, INPUT_PULLUP); - pinMode(49, INPUT_PULLUP); - pinMode(51, INPUT_PULLUP); - pinMode(53, INPUT_PULLUP); + pinMode(HOME1_PIN, INPUT_PULLUP); + pinMode(HOME2_PIN, INPUT_PULLUP); + pinMode(HOME3_PIN, INPUT_PULLUP); + pinMode(HOME4_PIN, INPUT_PULLUP); + pinMode(HOME5_PIN, INPUT_PULLUP); + pinMode(HOME6_PIN, INPUT_PULLUP); Serial.begin(57600); Serial2.begin(115200); @@ -140,7 +158,7 @@ void setup() { } delay (500); // wait for everything to finish - digitalWrite(39, HIGH); // set initial homing LED yellow right top + digitalWrite(LED_RIGHT_TOP_YELLOW, HIGH); // set initial homing LED yellow right top } @@ -153,23 +171,22 @@ void loop() { //start timed event previousMillis = currentMillis; - but1 = digitalRead(25); - but2 = digitalRead(27); - but3 = digitalRead(29); - but4 = digitalRead(31); + but1 = digitalRead(BUTTON1_PIN); + but2 = digitalRead(BUTTON2_PIN); + but3 = digitalRead(BUTTON3_PIN); + but4 = digitalRead(BUTTON4_PIN); - home1 = digitalRead(43); - home2 = digitalRead(45); - home3 = digitalRead(47); - home4 = digitalRead(49); - home5 = digitalRead(51); - home6 = digitalRead(53); - + home1 = digitalRead(HOME1_PIN); + home2 = digitalRead(HOME2_PIN); + home3 = digitalRead(HOME3_PIN); + home4 = digitalRead(HOME4_PIN); + home5 = digitalRead(HOME5_PIN); + home6 = digitalRead(HOME6_PIN); // *****************************right leg********************************** if (but4 == 0 && flag == 0) { - digitalWrite(39, LOW); + digitalWrite(LED_RIGHT_TOP_YELLOW, LOW); Serial.println("Right Motor 0"); requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; @@ -183,7 +200,7 @@ void loop() { odrive2.run_state(0, requested_state, false); // don't wait while (home4a == 1) { - home4 = digitalRead(49); + home4 = digitalRead(HOME4_PIN); if (home4 == 1) { previousFilterMillis = millis(); } @@ -215,7 +232,7 @@ void loop() { odrive2.run_state(1, requested_state, false); // don't wait while (home3a == 1) { - home3 = digitalRead(47); + home3 = digitalRead(HOME3_PIN); if (home3 == 1) { previousFilterMillis = millis(); } @@ -234,7 +251,7 @@ void loop() { Serial.println(home3Offset); odrive2.SetPosition(1, (home3Offset-(8192*2))); // back off one revolution flag = 1; - digitalWrite(37, HIGH); // Yellow top left + digitalWrite(LED_LEFT_TOP_YELLOW, HIGH); // Yellow top left } @@ -242,7 +259,7 @@ void loop() { // *********************************left leg************************************ else if (but3 == 0 && flag == 1) { - digitalWrite(37, LOW); + digitalWrite(LED_LEFT_TOP_YELLOW, LOW); Serial.println("Left Motor 0"); requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; @@ -256,7 +273,7 @@ void loop() { odrive1.run_state(0, requested_state, false); // don't wait while (home2a == 1) { - home2 = digitalRead(45); + home2 = digitalRead(HOME2_PIN); if (home2 == 1) { previousFilterMillis = millis(); } @@ -288,7 +305,7 @@ void loop() { odrive1.run_state(1, requested_state, false); // don't wait while (home1a == 1) { - home1 = digitalRead(43); + home1 = digitalRead(HOME1_PIN); if (home1 == 1) { previousFilterMillis = millis(); } @@ -308,14 +325,14 @@ void loop() { odrive1.SetPosition(1, (home1Offset-(8192*2))); // back off one revolution // time to boost the legs up - digitalWrite(37, HIGH); // yellow left - digitalWrite(39, HIGH); // yellow right + digitalWrite(LED_LEFT_TOP_YELLOW, HIGH); // yellow left + digitalWrite(LED_RIGHT_TOP_YELLOW, HIGH); // yellow right flag = 2; } else if (but4 == 0 || but3 == 0 && flag == 2) { - digitalWrite(37, LOW); - digitalWrite(39, LOW); + digitalWrite(LED_LEFT_TOP_YELLOW, LOW); + digitalWrite(LED_RIGHT_TOP_YELLOW, LOW); for (int axis = 0; axis < 2; ++axis) { Serial2 << "w axis" << axis << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast ODrive1 @@ -330,7 +347,7 @@ void loop() { odrive1.SetPosition(0, (home2Offset - 163840)); // move legs straight left odrive1.SetPosition(1, (home1Offset - 245760)); // move legs straight left - digitalWrite(35, HIGH); // white LED right - ready for undercarriage calibration + digitalWrite(LED_RIGHT_BOTTOM_WHITE, HIGH); // white LED right - ready for undercarriage calibration flag = 3; } @@ -339,7 +356,7 @@ void loop() { else if (but2 == 0 && flag == 3) { // bend right leg - digitalWrite(35, LOW); + digitalWrite(LED_RIGHT_BOTTOM_WHITE, LOW); odrive2.SetPosition(0, (home4Offset - 16384)); // move leg bent right odrive2.SetPosition(1, (home3Offset - 16384)); // move leg bent right delay (2000); // wait for leg to bend @@ -360,7 +377,7 @@ void loop() { // move undercarriage ODrive axis 1 under it hits the home switch 6 while (home6a == 1) { - home6 = digitalRead(53); + home6 = digitalRead(HOME6_PIN); if (home6 == 1) { previousFilterMillis = millis(); } @@ -392,14 +409,14 @@ void loop() { odrive2.SetPosition(0, (home4Offset - 163840)); // move legs straight right odrive2.SetPosition(1, (home3Offset - 245760)); // move legs straight right - digitalWrite(33, HIGH); // white LED left bottom - ready for the other side + digitalWrite(LED_LEFT_BOTTOM_WHITE, HIGH); // white LED left bottom - ready for the other side flag = 4; } //**************** do the other leg ***************** else if (but1 == 0 && flag == 4) { - digitalWrite(33, LOW); + digitalWrite(LED_LEFT_BOTTOM_WHITE, LOW); odrive1.SetPosition(0, (home2Offset - 16384)); // move leg bent left odrive1.SetPosition(1, (home1Offset - 16384)); // move leg bent left delay (2000); // wait for leg to bend @@ -420,7 +437,7 @@ void loop() { // move undercarriage ODrive axis 0 under it hits the home switch 5 while (home5a == 1) { - home5 = digitalRead(51); + home5 = digitalRead(HOME5_PIN); if (home5 == 1) { previousFilterMillis = millis(); } From 356bfc474505e65cc25c42f77af745470f073b29 Mon Sep 17 00:00:00 2001 From: fre-ber Date: Wed, 5 Sep 2018 23:20:54 +0200 Subject: [PATCH 3/3] Broke out the function to locate an end stop. This is a big one and I might have messed several things up here. Be careful to validate that the actual axis and pins are as they should. Also consider if any of the differences between the undercarriage calibration and the leg calibration that I removed really need to be there and add them in in a way that makes sense for both cases. --- Part8/Slave01/Slave01.ino | 441 ++++++++++++-------------------------- 1 file changed, 142 insertions(+), 299 deletions(-) diff --git a/Part8/Slave01/Slave01.ino b/Part8/Slave01/Slave01.ino index 637073e..a236f3e 100644 --- a/Part8/Slave01/Slave01.ino +++ b/Part8/Slave01/Slave01.ino @@ -56,32 +56,19 @@ int but2; // right bottom int but3; // left top int but4; // right top -int home1 = 1; -int home1a = 1; -int home2 = 1; -int home2a = 1; -int home3 = 1; -int home3a = 1; -int home4 = 1; -int home4a = 1; -int home5 = 1; -int home5a = 1; -int home6 = 1; -int home6a = 1; - -long home1Offset; -long home2Offset; -long home3Offset; -long home4Offset; -long home5Offset; -long home6Offset; - -long home1Home; -long home2Home; -long home3Home; -long home4Home; -long home5Home; -long home6Home; +enum HomeSwitchIndex +{ + HOME_SWITCH_LEFT_ELBOW_X, + HOME_SWITCH_LEFT_SHOULDER_X, + HOME_SWITCH_RIGHT_ELBOW_X, + HOME_SWITCH_RIGHT_SHOULDER_X, + HOME_SWITCH_LEFT_SHOULDER_Y, + HOME_SWITCH_RIGHT_SHOULDER_Y +}; +int homeSwitchState[6] = {1, 1, 1, 1, 1, 1}; +int homeSwitchStateFiltered[6] = {1, 1, 1, 1, 1, 1}; +long homeOffset[6]; +long homePos[6]; int flag = 0; @@ -102,24 +89,21 @@ double shoulderLFiltered2; double elbowRFiltered; double elbowLFiltered; -void setup() { - +void setup() +{ // LEDs - pinMode(LED_LEFT_BOTTOM_WHITE, OUTPUT); // white left bottom pinMode(LED_RIGHT_BOTTOM_WHITE, OUTPUT); // white right bottom pinMode(LED_LEFT_TOP_YELLOW, OUTPUT); // yellow left top pinMode(LED_RIGHT_TOP_YELLOW, OUTPUT); // yellow right top // control panel buttons - pinMode(BUTTON1_PIN, INPUT_PULLUP); // left bottom pinMode(BUTTON2_PIN, INPUT_PULLUP); // right bottom pinMode(BUTTON3_PIN, INPUT_PULLUP); // left top pinMode(BUTTON4_PIN, INPUT_PULLUP); // right top // home switches - pinMode(HOME1_PIN, INPUT_PULLUP); pinMode(HOME2_PIN, INPUT_PULLUP); pinMode(HOME3_PIN, INPUT_PULLUP); @@ -142,339 +126,149 @@ void setup() { // ***set mtor parameters for initial setup*** // right leg - for (int axis = 0; axis < 2; ++axis) { + for (int axis = 0; axis < 2; ++axis) + { Serial3 << "w axis" << axis << ".controller.config.vel_limit " << 22000.0f << '\n'; Serial3 << "w axis" << axis << ".motor.config.current_lim " << 20.0f << '\n'; } // left leg - for (int axis = 0; axis < 2; ++axis) { + for (int axis = 0; axis < 2; ++axis) + { Serial2 << "w axis" << axis << ".controller.config.vel_limit " << 22000.0f << '\n'; Serial2 << "w axis" << axis << ".motor.config.current_lim " << 20.0f << '\n'; } // undercariage - for (int axis = 0; axis < 2; ++axis) { + for (int axis = 0; axis < 2; ++axis) + { odrive_serial << "w axis" << axis << ".controller.config.vel_limit " << 22000.0f << '\n'; odrive_serial << "w axis" << axis << ".motor.config.current_lim " << 20.0f << '\n'; } delay (500); // wait for everything to finish digitalWrite(LED_RIGHT_TOP_YELLOW, HIGH); // set initial homing LED yellow right top - } -void loop() { - - unsigned long currentMillis = millis(); - if (currentMillis - previousMillis < interval) { +void loop() +{ + unsigned long currentMillis = millis(); + if (currentMillis - previousMillis < interval) + { return; - } - //start timed event - previousMillis = currentMillis; + } + + //start timed event + previousMillis = currentMillis; but1 = digitalRead(BUTTON1_PIN); but2 = digitalRead(BUTTON2_PIN); but3 = digitalRead(BUTTON3_PIN); but4 = digitalRead(BUTTON4_PIN); - - home1 = digitalRead(HOME1_PIN); - home2 = digitalRead(HOME2_PIN); - home3 = digitalRead(HOME3_PIN); - home4 = digitalRead(HOME4_PIN); - home5 = digitalRead(HOME5_PIN); - home6 = digitalRead(HOME6_PIN); + + homeSwitchState[HOME_SWITCH_LEFT_ELBOW_X] = digitalRead(HOME1_PIN); + homeSwitchState[HOME_SWITCH_LEFT_SHOULDER_X] = digitalRead(HOME2_PIN); + homeSwitchState[HOME_SWITCH_RIGHT_ELBOW_X] = digitalRead(HOME3_PIN); + homeSwitchState[HOME_SWITCH_RIGHT_SHOULDER_X] = digitalRead(HOME4_PIN); + homeSwitchState[HOME_SWITCH_LEFT_SHOULDER_Y] = digitalRead(HOME5_PIN); + homeSwitchState[HOME_SWITCH_RIGHT_SHOULDER_Y] = digitalRead(HOME6_PIN); // *****************************right leg********************************** - if (but4 == 0 && flag == 0) { + if (but4 == 0 && flag == 0) + { digitalWrite(LED_RIGHT_TOP_YELLOW, LOW); - - Serial.println("Right Motor 0"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(0, requested_state, false); // don't wait - - while (home4a == 1) { - home4 = digitalRead(HOME4_PIN); - if (home4 == 1) { - previousFilterMillis = millis(); - } - else if (home4 == 0 && millis() - previousFilterMillis > filterTime) { - home4a = 0; - } - // move motor 0 - odrive2.SetVelocity(0, 10000); - } - // stop motor 0 - odrive2.SetVelocity(0, 0); - delay(300); - //save zero position and back off two revolutions - Serial3 << "r axis" << 0 << ".encoder.pos_estimate\n"; - home4Offset = odrive2.readInt(); - Serial.println(home4Offset); - odrive2.SetPosition(0, (home4Offset-(8192*2))); // back off two revolutions - delay (500); // wait for that to properly finish - - Serial.println("Right Motor 1"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive2.run_state(1, requested_state, false); // don't wait - - while (home3a == 1) { - home3 = digitalRead(HOME3_PIN); - if (home3 == 1) { - previousFilterMillis = millis(); - } - else if (home3 == 0 && millis() - previousFilterMillis > filterTime) { - home3a = 0; - } - // move motor 1 - odrive2.SetVelocity(1, 10000); - } - //stop motor 1 - odrive2.SetVelocity(1, 0); - delay(300); - //save zero position and back off two revolutions - Serial3 << "r axis" << 1 << ".encoder.pos_estimate\n"; - home3Offset = odrive2.readInt(); - Serial.println(home3Offset); - odrive2.SetPosition(1, (home3Offset-(8192*2))); // back off one revolution + FindStop (odrive2, 0, 10000, HOME_SWITCH_RIGHT_SHOULDER_X, HOME4_PIN, "Right"); + FindStop (odrive2, 1, 10000, HOME_SWITCH_RIGHT_ELBOW_X, HOME3_PIN, "Right"); flag = 1; digitalWrite(LED_LEFT_TOP_YELLOW, HIGH); // Yellow top left } - - // *********************************left leg************************************ - else if (but3 == 0 && flag == 1) { + else if (but3 == 0 && flag == 1) + { digitalWrite(LED_LEFT_TOP_YELLOW, LOW); - - Serial.println("Left Motor 0"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(0, requested_state, false); // don't wait - - while (home2a == 1) { - home2 = digitalRead(HOME2_PIN); - if (home2 == 1) { - previousFilterMillis = millis(); - } - else if (home2 == 0 && millis() - previousFilterMillis > filterTime) { - home2a = 0; - } - // move motor 0 - odrive1.SetVelocity(0, 10000); - } - // stop motor 0 - odrive1.SetVelocity(0, 0); - delay(300); - //save zero position and back off two revolutions - Serial2 << "r axis" << 0 << ".encoder.pos_estimate\n"; - home2Offset = odrive1.readInt(); - Serial.println(home2Offset); - odrive1.SetPosition(0, (home2Offset-(8192*2))); // back off two revolutions - delay (500); // wait for that to properly finish - - Serial.println("Left Motor 1"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive1.run_state(1, requested_state, false); // don't wait - - while (home1a == 1) { - home1 = digitalRead(HOME1_PIN); - if (home1 == 1) { - previousFilterMillis = millis(); - } - else if (home1 == 0 && millis() - previousFilterMillis > filterTime) { - home1a = 0; - } - // move motor 1 - odrive1.SetVelocity(1, 10000); - } - //stop motor 1 - odrive1.SetVelocity(1, 0); - delay(300); - //save zero position and back off two revolutions - Serial2 << "r axis" << 1 << ".encoder.pos_estimate\n"; - home1Offset = odrive1.readInt(); - Serial.println(home1Offset); - odrive1.SetPosition(1, (home1Offset-(8192*2))); // back off one revolution + + FindStop (odrive1, 0, 10000, HOME_SWITCH_LEFT_SHOULDER_X, HOME2_PIN, "Left"); + FindStop (odrive1, 1, 10000, HOME_SWITCH_LEFT_ELBOW_X, HOME1_PIN, "Left"); // time to boost the legs up digitalWrite(LED_LEFT_TOP_YELLOW, HIGH); // yellow left digitalWrite(LED_RIGHT_TOP_YELLOW, HIGH); // yellow right flag = 2; } + - else if (but4 == 0 || but3 == 0 && flag == 2) { - digitalWrite(LED_LEFT_TOP_YELLOW, LOW); - digitalWrite(LED_RIGHT_TOP_YELLOW, LOW); - - for (int axis = 0; axis < 2; ++axis) { - Serial2 << "w axis" << axis << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast ODrive1 - } - for (int axis = 0; axis < 2; ++axis) { - Serial3 << "w axis" << axis << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast ODrive2 - } + else if (but4 == 0 || but3 == 0 && flag == 2) + { + digitalWrite(LED_LEFT_TOP_YELLOW, LOW); + digitalWrite(LED_RIGHT_TOP_YELLOW, LOW); + for (int axis = 0; axis < 2; ++axis) + { + Serial2 << "w axis" << axis << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast ODrive1 + } + for (int axis = 0; axis < 2; ++axis) + { + Serial3 << "w axis" << axis << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast ODrive2 + } - odrive2.SetPosition(0, (home4Offset - 163840)); // move legs straight right - odrive2.SetPosition(1, (home3Offset - 245760)); // move legs straight right + odrive2.SetPosition(0, (homeOffset[HOME_SWITCH_RIGHT_SHOULDER_X] - 163840)); // move legs straight right + odrive2.SetPosition(1, (homeOffset[HOME_SWITCH_RIGHT_ELBOW_X] - 245760)); // move legs straight right - odrive1.SetPosition(0, (home2Offset - 163840)); // move legs straight left - odrive1.SetPosition(1, (home1Offset - 245760)); // move legs straight left + odrive1.SetPosition(0, (homeOffset[HOME_SWITCH_LEFT_SHOULDER_X] - 163840)); // move legs straight left + odrive1.SetPosition(1, (homeOffset[HOME_SWITCH_LEFT_ELBOW_X] - 245760)); // move legs straight left - digitalWrite(LED_RIGHT_BOTTOM_WHITE, HIGH); // white LED right - ready for undercarriage calibration - flag = 3; + digitalWrite(LED_RIGHT_BOTTOM_WHITE, HIGH); // white LED right - ready for undercarriage calibration + flag = 3; } - // *****************************leg motor undercarriage************************************** - - else if (but2 == 0 && flag == 3) { + + else if (but2 == 0 && flag == 3) + { // bend right leg digitalWrite(LED_RIGHT_BOTTOM_WHITE, LOW); - odrive2.SetPosition(0, (home4Offset - 16384)); // move leg bent right - odrive2.SetPosition(1, (home3Offset - 16384)); // move leg bent right + odrive2.SetPosition(0, (homeOffset[HOME_SWITCH_RIGHT_SHOULDER_X] - 16384)); // move leg bent right + odrive2.SetPosition(1, (homeOffset[HOME_SWITCH_RIGHT_ELBOW_X] - 16384)); // move leg bent right delay (2000); // wait for leg to bend - - // calibrate right leg undercarriage, ODrive axis 1 - Serial.println("Right Leg undercarriage"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(1, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 1 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(1, requested_state, false); // don't wait - delay (500); // wait for that to properly finish - - // move undercarriage ODrive axis 1 under it hits the home switch 6 - - while (home6a == 1) { - home6 = digitalRead(HOME6_PIN); - if (home6 == 1) { - previousFilterMillis = millis(); - } - else if (home6 == 0 && millis() - previousFilterMillis > filterTime) { - home6a = 0; - } - // move motor 1 - odrive0.SetVelocity(1, -10000); - } - //stop motor 1 - odrive0.SetVelocity(1, 0); - - //save zero position - odrive_serial << "r axis" << 1 << ".encoder.pos_estimate\n"; - home6Offset = odrive0.readInt(); - Serial.print(home6Offset); - Serial.print(" , "); - while ((home6Offset < -122000) || (home6Offset > -52000)) { // filter because software serial is a bit rubbish - odrive_serial << "r axis" << 1 << ".encoder.pos_estimate\n"; - home6Offset = odrive0.readInt(); - } - Serial.println(home6Offset); // print filtered value - + + FindStop (odrive0, 1, -10000, HOME_SWITCH_RIGHT_SHOULDER_Y, HOME6_PIN, "Right Leg undercarriage"); + /* TODO: Should this be done? odrive_serial << "w axis" << 1 << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast for Odrive 0 / axis 1 odrive0.SetPosition(1, (home6Offset+87654)); // back off 25mm delay(3000); // wait for leg to move out again - + */ // put leg straight again - odrive2.SetPosition(0, (home4Offset - 163840)); // move legs straight right - odrive2.SetPosition(1, (home3Offset - 245760)); // move legs straight right + odrive2.SetPosition(0, (homeOffset[HOME_SWITCH_RIGHT_SHOULDER_X] - 163840)); // move legs straight right + odrive2.SetPosition(1, (homeOffset[HOME_SWITCH_RIGHT_ELBOW_X] - 245760)); // move legs straight right digitalWrite(LED_LEFT_BOTTOM_WHITE, HIGH); // white LED left bottom - ready for the other side flag = 4; - } - + } //**************** do the other leg ***************** - else if (but1 == 0 && flag == 4) { + else if (but1 == 0 && flag == 4) + { digitalWrite(LED_LEFT_BOTTOM_WHITE, LOW); - odrive1.SetPosition(0, (home2Offset - 16384)); // move leg bent left - odrive1.SetPosition(1, (home1Offset - 16384)); // move leg bent left + odrive1.SetPosition(0, (homeOffset[HOME_SWITCH_LEFT_SHOULDER_X] - 16384)); // move leg bent left + odrive1.SetPosition(1, (homeOffset[HOME_SWITCH_LEFT_ELBOW_X] - 16384)); // move leg bent left delay (2000); // wait for leg to bend - // calibrate right leg undercarriage, ODrive axis 0 - Serial.println("Right Leg undercarriage"); - requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(0, requested_state, true); - requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; - Serial << "Axis" << 0 << ": Requesting state " << requested_state << '\n'; - odrive0.run_state(0, requested_state, false); // don't wait - delay (500); // wait for that to properly finish - - // move undercarriage ODrive axis 0 under it hits the home switch 5 - - while (home5a == 1) { - home5 = digitalRead(HOME5_PIN); - if (home5 == 1) { - previousFilterMillis = millis(); - } - else if (home5 == 0 && millis() - previousFilterMillis > filterTime) { - home5a = 0; - } - // move motor 0 - odrive0.SetVelocity(0, -10000); - } - - //stop motor 0 - odrive0.SetVelocity(0, 0); - - //save zero position - odrive_serial << "r axis" << 0 << ".encoder.pos_estimate\n"; - home5Offset = odrive0.readInt(); - Serial.print(home5Offset); - Serial.print(" , "); - while ((home5Offset < -122000) || (home5Offset > -52000)) { // filter because software serial is a bit rubbish - odrive_serial << "r axis" << 0 << ".encoder.pos_estimate\n"; - home5Offset = odrive0.readInt(); - } - Serial.println(home5Offset); // print filtered value - + FindStop (odrive0, 0, -10000, HOME_SWITCH_LEFT_SHOULDER_Y, HOME5_PIN, "Left Leg undercarriage"); + /* TODO: Should this be done? odrive_serial << "w axis" << 0 << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast for Odrive 0 / axis 0 odrive0.SetPosition(0, (home6Offset+87654)); // back off 25mm delay(3000); // wait for leg to move out again - + */ // put leg straight again - odrive1.SetPosition(0, (home2Offset - 163840)); // move legs straight left - odrive1.SetPosition(1, (home1Offset - 245760)); // move legs straight left + odrive1.SetPosition(0, (homeOffset[HOME_SWITCH_LEFT_SHOULDER_X] - 163840)); // move legs straight left + odrive1.SetPosition(1, (homeOffset[HOME_SWITCH_LEFT_ELBOW_X] - 245760)); // move legs straight left delay(3000); // wait for the leg to be straight flag = 5; // proceed to next stage } - if (flag == 5) { // main output to leg starts here + if (flag == 5) + { // main output to leg starts here // ***********serial receive from master*************** @@ -495,20 +289,69 @@ void loop() { shoulderRFiltered2 = shoulderRFiltered*3490; // work out encoder counts per milimeter shoulderLFiltered2 = shoulderLFiltered*3490; // work out encoder counts per milimeter - home2Home = home2Offset - 163840; // the current position it's at - home4Home = home4Offset - 163840; // the current position it's at + homePos[HOME_SWITCH_LEFT_SHOULDER_X] = homeOffset[HOME_SWITCH_LEFT_SHOULDER_X] - 163840; // the current position it's at + homePos[HOME_SWITCH_RIGHT_SHOULDER_X] = homeOffset[HOME_SWITCH_RIGHT_SHOULDER_X] - 163840; // the current position it's at - odrive2.SetPosition(0, (home4Home + shoulderRFiltered2)); // use the test remote data to move the actuator - odrive1.SetPosition(0, (home2Home + shoulderLFiltered2)); // use the test remote data to move the actuator + odrive2.SetPosition(0, (homePos[HOME_SWITCH_LEFT_SHOULDER_X] + shoulderRFiltered2)); // use the test remote data to move the actuator + odrive1.SetPosition(0, (homePos[HOME_SWITCH_RIGHT_SHOULDER_X] + shoulderLFiltered2)); // use the test remote data to move the actuator } // end of main output to leg +} + + + + +void FindStop (ODriveArduino odrive, int axis, long velocity, int homeSwitchIndex, int homeSwitchPin, String logPrefix) +{ + Serial << logPrefix << " Motor 0\n"; + requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; + Serial << "Axis" << axis << ": Requesting state " << requested_state << '\n'; + odrive.run_state(axis, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; + Serial << "Axis" << axis << ": Requesting state " << requested_state << '\n'; + odrive.run_state(axis, requested_state, true); + requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; + Serial << "Axis" << axis << ": Requesting state " << requested_state << '\n'; + odrive.run_state(axis, requested_state, false); // don't wait + + // TODO: If necessary, add a parameter that determines if we wait for 500 milliseconds here. As was done for the undercarriage calibration. + + while (homeSwitchStateFiltered[homeSwitchIndex] == 1) + { + homeSwitchState[homeSwitchIndex] = digitalRead(homeSwitchPin); + if (homeSwitchState[homeSwitchIndex] == 1) + { + previousFilterMillis = millis(); + } + else if (homeSwitchState[homeSwitchIndex] == 0 && millis() - previousFilterMillis > filterTime) + { + homeSwitchStateFiltered[homeSwitchIndex] = 0; + } + // move motor 0 + odrive.SetVelocity(axis, velocity); + } + // stop motor 0 + odrive.SetVelocity(axis, 0); + delay(300); + //save zero position and back off two revolutions + Serial3 << "Axis" << axis << ".encoder.pos_estimate\n"; + homeOffset[homeSwitchIndex] = odrive.readInt(); + Serial.println(homeOffset[homeSwitchIndex]); + + // TODO: If necessary, add a parameter that determines if we need to "filter" the position. + // TODO: If necessary, change the velocity limit to 48000.0f here? + + odrive.SetPosition(axis, (homeOffset[homeSwitchIndex] - (8192 * 2))); // back off two revolutions + //TODO: If necessary, add a parameter to alter the time we wait as it was different for the undercarriage calibration. + delay (500); // wait for that to properly finish } //***************filter joint motions***************** -double filter(double lengthOrig, double currentValue) { +double filter(double lengthOrig, double currentValue) +{ double filter = 15; double lengthFiltered = (lengthOrig + (currentValue * filter)) / (filter + 1); return lengthFiltered;