ichinoseki_Bteam_2019
/
ArmNode
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: IMU SPI_Encoder mbed Path PID Motor Inertia ros_lib_kinetic Kinematics Mycan
Diff: main.cpp
- Revision:
- 5:a6d22b209980
- Parent:
- 4:7c19daf24a7d
--- a/main.cpp Thu Sep 26 13:50:59 2019 +0000
+++ b/main.cpp Thu Sep 26 15:15:27 2019 +0000
@@ -8,6 +8,7 @@
}
ros::Subscriber<std_msgs::Float32MultiArray> sub("bath_towel", callback);
*/
+
void gurdTorque()
{
float max_torque[3] = {0.216, 1.256, 0.216};
@@ -108,6 +109,16 @@
else torque_flag = false;
}
+void accel()
+{
+ for (int i = 0; i < 3; i++)
+ position_convergence[i] = pidPosition[i].compute();
+ kinematics.computeJacobian();
+ for (int i = 0; i < 3; i++)
+ velocity_convergence[i] = pidVelocity[i].compute();
+}
+
+
void looping()
{
imu.computeAngles();
@@ -140,7 +151,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 80);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetT[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if ((passtime <= totalt - 2)) {
for (int i = 0; i < 3; i++)
@@ -148,7 +161,9 @@
kinematics.HorizontalVertical(HORIZONTAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetT[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if ((passtime <= totalt - 1)) {
for (int i = 0; i < 3; i++)
@@ -156,7 +171,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetT[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
can.setI(SERVO_NODE, 1, 20);
@@ -164,7 +181,11 @@
passtime = 0;
convergence++;
}
- else torque_flag = false;
+ else {
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = pidVelocity[i].output + pidPosition[i].output;
+ }
passtime += delta_t;
break;
case 22:
@@ -179,7 +200,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetF[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
can.setI(SERVO_NODE, 1, 20);
@@ -192,7 +215,11 @@
pidVelocity[i].target = &targetX[i].df;
}
}
- else torque_flag = false;
+ else {
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = pidVelocity[i].output + pidPosition[i].output;
+ }
}
else if (!arm_flag) {
if ((passtime <= total - 1)) {
@@ -201,7 +228,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetX[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
can.setI(SERVO_NODE, 1, 70);
@@ -214,7 +243,11 @@
pidVelocity[i].target = &targetHang[i].df;
}
}
- else torque_flag = false;
+ else {
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = pidVelocity[i].output + pidPosition[i].output;
+ }
}
else hangHanger(0);
passtime += delta_t;
@@ -227,7 +260,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetX2[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
can.setI(SERVO_NODE, 1, 70);
@@ -240,7 +275,11 @@
pidVelocity[i].target = &targetHang[i].df;
}
}
- else torque_flag = false;
+ else {
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = pidVelocity[i].output + pidPosition[i].output;
+ }
}
else hangHanger(0);
passtime += delta_t;
@@ -253,7 +292,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetX3[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
can.setI(SERVO_NODE, 1, 70);
@@ -266,7 +307,11 @@
pidVelocity[i].target = &targetHang[i].df;
}
}
- else torque_flag = false;
+ else {
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = pidVelocity[i].output + pidPosition[i].output;
+ }
}
else hangHanger(0);
passtime += delta_t;
@@ -279,7 +324,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetX4[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
can.setI(SERVO_NODE, 1, 70);
@@ -292,7 +339,11 @@
pidVelocity[i].target = &targetHang[i].df;
}
}
- else torque_flag = false;
+ else {
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = pidVelocity[i].output + pidPosition[i].output;
+ }
}
else hangHanger(0);
passtime += delta_t;
@@ -305,7 +356,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetR[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
can.setI(SERVO_NODE, 1, 70);
@@ -318,7 +371,11 @@
pidVelocity[i].target = &targetX[i].df;
}
}
- else torque_flag = false;
+ else {
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = pidVelocity[i].output + pidPosition[i].output;
+ }
}
else if (!arm_flag) {
if ((passtime <= total - 1)) {
@@ -328,7 +385,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetX[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
can.setI(SERVO_NODE, 1, 70);
@@ -341,7 +400,11 @@
pidVelocity[i].target = &targetHang[i].df;
}
}
- else torque_flag = false;
+ else {
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = pidVelocity[i].output + pidPosition[i].output;
+ }
}
else hangHanger(1);
passtime += delta_t;
@@ -355,7 +418,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetX2[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
can.setI(SERVO_NODE, 1, 70);
@@ -368,7 +433,11 @@
pidVelocity[i].target = &targetHang[i].df;
}
}
- else torque_flag = false;
+ else {
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = pidVelocity[i].output + pidPosition[i].output;
+ }
}
else hangHanger(1);
passtime += delta_t;
@@ -382,7 +451,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetX3[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
can.setI(SERVO_NODE, 1, 70);
@@ -395,7 +466,9 @@
pidVelocity[i].target = &targetHang[i].df;
}
}
- else torque_flag = false;
+ else accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = pidVelocity[i].output + pidPosition[i].output;
}
else hangHanger(1);
passtime += delta_t;
@@ -409,7 +482,9 @@
kinematics.HorizontalVertical(VERTICAL);
can.setI(SERVO_NODE, 1, 20);
can.send();
- torque_flag = true;
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = targetX4[i].ddf + pidVelocity[i].output + pidPosition[i].output;
}
else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
can.setI(SERVO_NODE, 1, 70);
@@ -422,7 +497,11 @@
pidVelocity[i].target = &targetHang[i].df;
}
}
- else torque_flag = false;
+ else {
+ accel();
+ for (int i = 0; i < 3; i++)
+ a_result[i] = pidVelocity[i].output + pidPosition[i].output;
+ }
}
else hangHanger(1);
passtime += delta_t;
@@ -430,21 +509,13 @@
}
-
- for (int i = 0; i < 3; i++)
- position_convergence[i] = pidPosition[i].compute();
- kinematics.computeJacobian();
- for (int i = 0; i < 3; i++)
- velocity_convergence[i] = pidVelocity[i].compute();
- for (int i = 0; i < 3; i++)
- a_result[i] = targetX[i].ddf + pidVelocity[i].output + pidPosition[i].output;
kinematics.computeAcceleration();
inertia.calculate();
float ratio[4] = {335.28, 104, 100, 71};
- if (enc_error || !torque_flag){
+ if (enc_error){
torque[0] = inertia.gravity[0] / (ratio[0] * 0.5);
torque[1] = inertia.gravity[1] / (ratio[1] * 0.6);
torque[2] = inertia.gravity[2] / (ratio[2] * 0.6);