diff --git a/Part8/Slave01/Slave01.ino b/Part8/Slave01/Slave01.ino index 917aade..a236f3e 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 @@ -38,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; @@ -84,30 +89,27 @@ double shoulderLFiltered2; double elbowRFiltered; double elbowLFiltered; -void setup() { - +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); @@ -124,375 +126,232 @@ 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(39, HIGH); // set initial homing LED yellow right top - + digitalWrite(LED_RIGHT_TOP_YELLOW, HIGH); // set initial homing LED yellow right top } -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 - } +void loop() +{ + unsigned long currentMillis = millis(); + if (currentMillis - previousMillis < interval) + { + return; + } - - - // *********************************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; - } - + //start timed event + previousMillis = currentMillis; - // *****************************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); - } + but1 = digitalRead(BUTTON1_PIN); + but2 = digitalRead(BUTTON2_PIN); + but3 = digitalRead(BUTTON3_PIN); + but4 = digitalRead(BUTTON4_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) + { + digitalWrite(LED_RIGHT_TOP_YELLOW, LOW); + 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) + { + digitalWrite(LED_LEFT_TOP_YELLOW, LOW); + + 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 + } + + 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, (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; + } + + // *****************************leg motor undercarriage************************************** + + else if (but2 == 0 && flag == 3) + { + // bend right leg + digitalWrite(LED_RIGHT_BOTTOM_WHITE, LOW); + 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 + + 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, (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) + { + digitalWrite(LED_LEFT_BOTTOM_WHITE, LOW); + 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 + + 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, (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 + + // ***********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); + + Serial.print(" , "); - ET3.receiveData(); - - Serial.print(mydata_back.shoulderR); // print original test value - Serial.print(" , "); - shoulderRFiltered = filter(mydata_back.shoulderR, shoulderRFiltered); - Serial.print(shoulderRFiltered); + Serial.print(mydata_back.shoulderL); // print original test value + Serial.print(" , "); + shoulderLFiltered = filter(mydata_back.shoulderL, shoulderLFiltered); + Serial.println(shoulderLFiltered); - Serial.print(" , "); + shoulderRFiltered2 = shoulderRFiltered*3490; // work out encoder counts per milimeter + shoulderLFiltered2 = shoulderLFiltered*3490; // work out encoder counts per milimeter - Serial.print(mydata_back.shoulderL); // print original test value - Serial.print(" , "); - shoulderLFiltered = filter(mydata_back.shoulderL, shoulderLFiltered); - Serial.println(shoulderLFiltered); + 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, (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 +} - 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 +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]); - } // end of timed event - + // 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;