Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Cleaning up slave data types. #16

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Cleaning up slave data types. #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3a37ec3
Add files via upload
Rerouter Nov 6, 2018
dd090a6
Cleaning Data Types
Rerouter Nov 6, 2018
2d16b40
Merge pull request #1 from Rerouter/Rerouter-patch-4
Rerouter Nov 6, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
181 changes: 79 additions & 102 deletions Part12/Slave01/Slave01.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,30 +33,30 @@ SLAVE1_DATA_STRUCTURE mydata_back;

// Slave 1 - Back End - bottom ctrl panel!

int but1; // left bottom
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;
byte but1; // left bottom
byte but2; // right bottom
byte but3; // left top
byte but4; // right top

byte home1 = 1;
byte home1a = 1;
byte home2 = 1;
byte home2a = 1;
byte home3 = 1;
byte home3a = 1;
byte home4 = 1;
byte home4a = 1;
byte home5 = 1;
byte home5a = 1;
byte home6 = 1;
byte home6a = 1;

int home1Offset;
int home2Offset;
int home3Offset;
int home4Offset;
int home5Offset;
int home6Offset;

long home1Home;
long home2Home;
Expand All @@ -65,40 +65,40 @@ long home4Home;
long home5Home;
long home6Home;

int flag = 0;
byte flag = 0;

int motorSpeedFlag = 0;
byte motorSpeedFlag = 0;

int requested_state;

unsigned long previousFilterMillis; // digital pin filter
int filterTime = 200;
uint16_t filterTime = 200;

unsigned long previousStartupmillis;
int startupFlag = 0;
byte startupFlag = 0;

unsigned long previousMillis = 0;
const long interval = 10;

double hipRFiltered; // value in mm
double hipLFiltered;
double hipRFiltered2; // value in encoder counts
double hipLFiltered2;
double shoulderRFiltered; // value in mm
double shoulderLFiltered;
double shoulderRFiltered2; // value in encoder counts
double shoulderLFiltered2;
double elbowRFiltered; // value in mm
double elbowLFiltered;
double elbowRFiltered2; // value in encoder counts
double elbowLFiltered2;

double hipROutput; // actual output in encoder counts
double hipLOutput;
double shoulderROutput;
double shoulderLOutput;
double elbowROutput;
double elbowLOutput;
float hipRFiltered; // value in mm
float hipLFiltered;
float hipRFiltered2; // value in encoder counts
float hipLFiltered2;
float shoulderRFiltered; // value in mm
float shoulderLFiltered;
float shoulderRFiltered2; // value in encoder counts
float shoulderLFiltered2;
float elbowRFiltered; // value in mm
float elbowLFiltered;
float elbowRFiltered2; // value in encoder counts
float elbowLFiltered2;

float hipROutput; // actual output in encoder counts
float hipLOutput;
float shoulderROutput;
float shoulderLOutput;
float elbowROutput;
float elbowLOutput;

void setup() {

Expand Down Expand Up @@ -140,24 +140,23 @@ void setup() {
// ***set mtor parameters for initial setup***

// right leg
for (int axis = 0; axis < 2; ++axis) {
for (byte 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 (byte 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 (byte 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(39, HIGH); // set initial homing LED yellow top right
}

void loop() {
Expand All @@ -170,7 +169,7 @@ void loop() {
but2 = digitalRead(27);
but3 = digitalRead(29);
but4 = digitalRead(31);

home1 = digitalRead(43);
home2 = digitalRead(45);
home3 = digitalRead(47);
Expand All @@ -179,12 +178,12 @@ void loop() {
home6 = digitalRead(53);


// *****************************right leg**********************************
// ********************* Right leg ****************

if (but4 == 0 && flag == 0) {
digitalWrite(39, LOW);

Serial.println("Right Motor 0");
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);
Expand Down Expand Up @@ -216,7 +215,8 @@ void loop() {
odrive2.SetPosition(0, (home4Offset-(8192*2))); // back off two revolutions
delay (500); // wait for that to properly finish

Serial.println("Right Motor 1");
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);
Expand Down Expand Up @@ -250,14 +250,12 @@ void loop() {
digitalWrite(37, HIGH); // Yellow top left
}



// *********************************left leg************************************
// ********************* left leg ****************

else if (but3 == 0 && flag == 1) {
digitalWrite(37, LOW);

Serial.println("Left Motor 0");
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);
Expand Down Expand Up @@ -325,17 +323,16 @@ void loop() {
// time to boost the legs up
digitalWrite(37, HIGH); // yellow left
digitalWrite(39, HIGH); // yellow right
flag = 2;
flag = 2;
}

else if (but4 == 0 || but3 == 0 && flag == 2) {
digitalWrite(37, LOW);
digitalWrite(39, LOW);
digitalWrite(39, LOW);

for (int axis = 0; axis < 2; ++axis) {
for (byte 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) {
for (byte axis = 0; axis < 2; ++axis) {
Serial3 << "w axis" << axis << ".controller.config.vel_limit " << 48000.0f << '\n'; // set motor speed to fast ODrive2
}

Expand All @@ -347,9 +344,7 @@ void loop() {
*/
digitalWrite(35, HIGH); // white LED right - ready for undercarriage calibration
flag = 3;

}


// *****************************leg motor undercarriage**************************************

Expand All @@ -358,11 +353,10 @@ void loop() {
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
Expand Down Expand Up @@ -411,12 +405,10 @@ void loop() {
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(1, (home3Offset - 245760)); // move legs straight right
*/

digitalWrite(33, HIGH); // white LED left bottom - ready for the other side
flag = 4;
Expand All @@ -430,9 +422,8 @@ void loop() {
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;
Expand All @@ -446,10 +437,8 @@ void loop() {
odrive0.run_state(0, requested_state, false); // don't wait
delay (500); // wait for that to properly finish



/* COMMENTING OUT - CALIBRATION IS DONE WITH MANUAL ALIGNMENT

// move undercarriage ODrive axis 0 under it hits the home switch 5

while (home5a == 1) {
Expand All @@ -463,7 +452,7 @@ void loop() {
// move motor 0
odrive0.SetVelocity(0, -10000);
}

//stop motor 0
odrive0.SetVelocity(0, 0);

Expand All @@ -482,15 +471,11 @@ void loop() {
odrive0.SetPosition(0, (home5Offset+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
}
Expand All @@ -514,7 +499,7 @@ void loop() {
}

// ***********serial receive from master***************
if (ET3.receiveData()) {
if (ET3.receiveData()) {

if(startupFlag == 0) {
previousStartupmillis = currentMillis; // start filter settling timer
Expand All @@ -528,8 +513,8 @@ void loop() {
mydata_back.hipR = constrain(mydata_back.hipR, -35,35);
mydata_back.hipL = constrain(mydata_back.hipL, -35,35);

shoulderRFiltered = filter(mydata_back.shoulderR, shoulderRFiltered);
shoulderLFiltered = filter(mydata_back.shoulderL, shoulderLFiltered);
shoulderRFiltered = filter(mydata_back.shoulderR, shoulderRFiltered);
shoulderLFiltered = filter(mydata_back.shoulderL, shoulderLFiltered);
elbowRFiltered = filter(mydata_back.elbowR, elbowRFiltered);
elbowLFiltered = filter(mydata_back.elbowL, elbowLFiltered);

Expand All @@ -538,11 +523,11 @@ void loop() {
shoulderRFiltered2 = shoulderRFiltered*3490; // work out encoder counts per milimeter
shoulderLFiltered2 = shoulderLFiltered*3490; // work out encoder counts per milimeter
elbowRFiltered2 = elbowRFiltered*3490; // work out encoder counts per milimeter
elbowLFiltered2 = elbowLFiltered*3490; // work out encoder counts per milimeter
elbowLFiltered2 = elbowLFiltered*3490; // work out encoder counts per milimeter

shoulderROutput = home4Offset - (715450 - shoulderRFiltered2); // use offset from homing minus the actual postion of the leg from the joint
elbowROutput = home3Offset - (715450 - elbowRFiltered2);
elbowROutput = home3Offset - (715450 - elbowRFiltered2);

shoulderLOutput = home2Offset - (715450 - shoulderLFiltered2); // use offset from homing minus the actual postion of the leg from the joint
elbowLOutput = home1Offset - (715450 - elbowLFiltered2);

Expand All @@ -569,10 +554,8 @@ void loop() {
odrive0.SetPosition(1, hipROutput); // output data to ODrive
}


} // end of receive data


} // end of main output to leg

} // end of timed event
Expand All @@ -583,14 +566,8 @@ void loop() {

//***************filter joint motions*****************

double filter(double lengthOrig, double currentValue) {
double filter = 40;
double lengthFiltered = (lengthOrig + (currentValue * filter)) / (filter + 1);
float filter(float lengthOrig, float currentValue) {
float filter = 40;
float lengthFiltered = (lengthOrig + (currentValue * filter)) / (filter + 1);
return lengthFiltered;
}






Loading