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:
- 4:7c19daf24a7d
- Parent:
- 3:e6d0abef5936
- Child:
- 5:a6d22b209980
diff -r e6d0abef5936 -r 7c19daf24a7d main.cpp
--- a/main.cpp Thu Sep 19 13:01:46 2019 +0000
+++ b/main.cpp Thu Sep 26 13:50:59 2019 +0000
@@ -10,6 +10,8 @@
*/
void gurdTorque()
{
+ float max_torque[3] = {0.216, 1.256, 0.216};
+
for(int i = 0; i < 3; i++) {
if(abs(torque[i]) > max_torque[i]) {
if(torque[i] > 0) torque[i] = max_torque[i];
@@ -18,63 +20,416 @@
}
}
+void hangHanger(bool minus)
+{
+ if ((passtime <= totalhang - 4)) {
+ if (!minus)
+ for (int i = 0; i < 3; i++)
+ targetHang[i].Calc(passtime, 0);
+ else {
+ targetHang[0].Calc(passtime, 1);
+ targetHang[1].Calc(passtime, 0);
+ targetHang[2].Calc(passtime, 0);
+ }
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 70);
+ can.send();
+ torque_flag = true;
+ }
+ else if ((passtime <= totalhang - 2)) {
+ if (!minus)
+ for (int i = 0; i < 3; i++)
+ targetHang[i].Calc(passtime, 0);
+ else {
+ targetHang[0].Calc(passtime, 1);
+ targetHang[1].Calc(passtime, 0);
+ targetHang[2].Calc(passtime, 0);
+ }
+ kinematics.HorizontalVertical(HORIZONTAL);
+ can.setI(SERVO_NODE, 1, 70);
+ can.send();
+ torque_flag = true;
+ }
+ else if ((passtime <= totalhang - 1)) {
+ if (!minus)
+ for (int i = 0; i < 3; i++)
+ targetHang[i].Calc(passtime, 0);
+ else {
+ targetHang[0].Calc(passtime, 1);
+ targetHang[1].Calc(passtime, 0);
+ targetHang[2].Calc(passtime, 0);
+ }
+ kinematics.HorizontalVertical(HORIZONTAL);
+ can.setI(SERVO_NODE, 1, 35);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ arm_flag = false;
+ passtime = 0;
+ kinematics.HorizontalVertical(HORIZONTAL);
+ switch (convergence) {
+ case 22:
+ case 30:
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetX2[i].f;
+ pidVelocity[i].target = &targetX2[i].df;
+ }
+ convergence += 2;
+ break;
+ case 24:
+ case 32:
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetX3[i].f;
+ pidVelocity[i].target = &targetX3[i].df;
+ }
+ convergence += 2;
+ break;
+ case 26:
+ case 34:
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetX4[i].f;
+ pidVelocity[i].target = &targetX4[i].df;
+ }
+ convergence += 2;
+ break;
+ case 28:
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetR[i].f;
+ pidVelocity[i].target = &targetR[i].df;
+ }
+ convergence += 2;
+ break;
+ }
+ //convergence++;
+ }
+ else torque_flag = false;
+}
+
void looping()
{
imu.computeAngles();
- for (int i = 0; i < 4; i++)
- encoder.getPosition(i);
+ for (int i = 0; i < 3; i++) {
+ if (encoder.getPosition(i)) {
+ enc_error = true;
+ enc_num = i;
+ break;
+ }
+ }
kinematics.computeKinematics();
- /*can.readI();
- convergence = can.get(INFO, 2);*/
+ can.readF();
+ if(can.get(INFO_ID, 0) > convergence) convergence = can.get(INFO_ID, 0);
- if (convergence >= 9)
+ if (convergence >= 37)
flag = false;
- if (flag)
+ else if (convergence >= 1)
{
- /*
switch (convergence) {
- case 3:
- if (passtime <= total - 1)
+ case 7:
+ case 9:
+ case 11:
+ case 13:
+ if ((passtime <= totalt - 6)) {
+ for (int i = 0; i < 3; i++)
+ targetT[i].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 80);
+ can.send();
+ torque_flag = true;
+ }
+ else if ((passtime <= totalt - 2)) {
for (int i = 0; i < 3; i++)
- targetX[i].Calc(passtime);
+ targetT[i].Calc(passtime, 0);
+ kinematics.HorizontalVertical(HORIZONTAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if ((passtime <= totalt - 1)) {
+ for (int i = 0; i < 3; i++)
+ targetT[i].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ passtime = 0;
+ convergence++;
+ }
+ else torque_flag = false;
+ passtime += delta_t;
break;
- case 7:
- if (passtime <= total - 1)
- for (int i = 0; i < 3; i++)
- targetX[i].Calc(passtime);
+ case 22:
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetF[i].f;
+ pidVelocity[i].target = &targetF[i].df;
+ }
+ if(!set_flag) {
+ if ((passtime <= totalf - 1)) {
+ for (int i = 0; i < 3; i++)
+ targetF[i].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ set_flag = true;
+ passtime = 0;
+ kinematics.HorizontalVertical(VERTICAL);
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetX[i].f;
+ pidVelocity[i].target = &targetX[i].df;
+ }
+ }
+ else torque_flag = false;
+ }
+ else if (!arm_flag) {
+ if ((passtime <= total - 1)) {
+ for (int i = 0; i < 3; i++)
+ targetX[i].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 70);
+ can.send();
+ arm_flag = true;
+ passtime = 0;
+ kinematics.HorizontalVertical(VERTICAL);
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetHang[i].f;
+ pidVelocity[i].target = &targetHang[i].df;
+ }
+ }
+ else torque_flag = false;
+ }
+ else hangHanger(0);
+ passtime += delta_t;
+ break;
+ case 24:
+ if (!arm_flag) {
+ if ((passtime <= total2 - 1)) {
+ for (int i = 0; i < 3; i++)
+ targetX2[i].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 70);
+ can.send();
+ arm_flag = true;
+ passtime = 0;
+ kinematics.HorizontalVertical(VERTICAL);
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetHang[i].f;
+ pidVelocity[i].target = &targetHang[i].df;
+ }
+ }
+ else torque_flag = false;
+ }
+ else hangHanger(0);
+ passtime += delta_t;
+ break;
+ case 26:
+ if (!arm_flag) {
+ if ((passtime <= total3 - 1)) {
+ for (int i = 0; i < 3; i++)
+ targetX3[i].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 70);
+ can.send();
+ arm_flag = true;
+ passtime = 0;
+ kinematics.HorizontalVertical(VERTICAL);
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetHang[i].f;
+ pidVelocity[i].target = &targetHang[i].df;
+ }
+ }
+ else torque_flag = false;
+ }
+ else hangHanger(0);
+ passtime += delta_t;
break;
- }*/
- if (passtime <= total - 1 && !inverse_flag) {
- for (int i = 0; i < 3; i++)
- targetX[i].Calc(passtime);
- kinematics.HorizontalVertical(VERTICAL);
- can.setI(SERVO_NODE, 1, 1);
- can.send();
- }
- else if (passtime <= total - 1 && inverse_flag) {
- for (int i = 0; i < 3; i++)
- targetX2[i].Calc(passtime);
- kinematics.HorizontalVertical(HORIZONTAL);
- can.setI(SERVO_NODE, 1, 0);
- can.send();
+ case 28:
+ if (!arm_flag) {
+ if ((passtime <= total4 - 1)) {
+ for (int i = 0; i < 3; i++)
+ targetX4[i].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 70);
+ can.send();
+ arm_flag = true;
+ passtime = 0;
+ kinematics.HorizontalVertical(VERTICAL);
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetHang[i].f;
+ pidVelocity[i].target = &targetHang[i].df;
+ }
+ }
+ else torque_flag = false;
+ }
+ else hangHanger(0);
+ passtime += delta_t;
+ break;
+ case 30:
+ if(!inverse_flag) {
+ if ((passtime <= totalr - 1)) {
+ for (int i = 0; i < 3; i++)
+ targetR[i].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 70);
+ can.send();
+ inverse_flag = true;
+ passtime = 0;
+ kinematics.HorizontalVertical(VERTICAL);
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetX[i].f;
+ pidVelocity[i].target = &targetX[i].df;
+ }
+ }
+ else torque_flag = false;
+ }
+ else if (!arm_flag) {
+ if ((passtime <= total - 1)) {
+ targetX[0].Calc(passtime, 1);
+ targetX[1].Calc(passtime, 0);
+ targetX[2].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 70);
+ can.send();
+ arm_flag = true;
+ passtime = 0;
+ kinematics.HorizontalVertical(VERTICAL);
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetHang[i].f;
+ pidVelocity[i].target = &targetHang[i].df;
+ }
+ }
+ else torque_flag = false;
+ }
+ else hangHanger(1);
+ passtime += delta_t;
+ break;
+ case 32:
+ if (!arm_flag) {
+ if ((passtime <= total2 - 1)) {
+ targetX2[0].Calc(passtime, 1);
+ targetX2[1].Calc(passtime, 0);
+ targetX2[2].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 70);
+ can.send();
+ arm_flag = true;
+ passtime = 0;
+ kinematics.HorizontalVertical(VERTICAL);
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetHang[i].f;
+ pidVelocity[i].target = &targetHang[i].df;
+ }
+ }
+ else torque_flag = false;
+ }
+ else hangHanger(1);
+ passtime += delta_t;
+ break;
+ case 34:
+ if (!arm_flag) {
+ if ((passtime <= total3 - 1)) {
+ targetX3[0].Calc(passtime, 1);
+ targetX3[1].Calc(passtime, 0);
+ targetX3[2].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 70);
+ can.send();
+ arm_flag = true;
+ passtime = 0;
+ kinematics.HorizontalVertical(VERTICAL);
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetHang[i].f;
+ pidVelocity[i].target = &targetHang[i].df;
+ }
+ }
+ else torque_flag = false;
+ }
+ else hangHanger(1);
+ passtime += delta_t;
+ break;
+ case 36:
+ if (!arm_flag) {
+ if ((passtime <= total4 - 1)) {
+ targetX4[0].Calc(passtime, 1);
+ targetX4[1].Calc(passtime, 0);
+ targetX4[2].Calc(passtime, 0);
+ kinematics.HorizontalVertical(VERTICAL);
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
+ torque_flag = true;
+ }
+ else if (position_convergence[0] && position_convergence[1] && position_convergence[2]) {
+ can.setI(SERVO_NODE, 1, 70);
+ can.send();
+ arm_flag = true;
+ passtime = 0;
+ kinematics.HorizontalVertical(VERTICAL);
+ for (int i = 0; i < 3; i++) {
+ pidPosition[i].target = &targetHang[i].f;
+ pidVelocity[i].target = &targetHang[i].df;
+ }
+ }
+ else torque_flag = false;
+ }
+ else hangHanger(1);
+ passtime += delta_t;
+ break;
+
}
- else if (position_convergence[0] && position_convergence[1] && position_convergence[2] && !inverse_flag) {
- can.setI(SERVO_NODE, 1, 0);
- can.send();
- inverse_flag = true;
- passtime = 0;
- for (int i = 0; i < 3; i++) {
- pidPosition[i].target = &targetX2[i].f;
- pidVelocity[i].target = &targetX2[i].df;
- }
- }
- else if (position_convergence[0] && position_convergence[1] && position_convergence[2] && inverse_flag) {
- can.setI(SERVO_NODE, 1, 1);
- can.send();
- }
+
for (int i = 0; i < 3; i++)
position_convergence[i] = pidPosition[i].compute();
@@ -83,43 +438,49 @@
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();
- if (convergence == 3) {
- torque[0] = (inertia.gravity[0] + kinematics.ddq[0] * inertia.i_moment[0]) / (ratio[0] * 0.5);
- torque[1] = (inertia.gravity[1] + kinematics.ddq[1] * inertia.i_moment[1]) / (ratio[1] * 0.6);
- torque[2] = (inertia.gravity[2] + kinematics.ddq[2] * inertia.i_moment[2]) / (ratio[2] * 0.6);
+ float ratio[4] = {335.28, 104, 100, 71};
+
+ if (enc_error || !torque_flag){
+ 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);
}
else {
torque[0] = (inertia.gravity[0] + kinematics.ddq[0] * inertia.i_moment[0]) / (ratio[0] * 0.5);
torque[1] = (inertia.gravity[1] + kinematics.ddq[1] * inertia.i_moment[1]) / (ratio[1] * 0.6);
torque[2] = (inertia.gravity[2] + kinematics.ddq[2] * inertia.i_moment[2]) / (ratio[2] * 0.6);
- /*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);*/
}
gurdTorque();
+
+ float voltage = 12.2;
+ float resistance[3] = {0.16842, 0.11538, 0.16842};
+ float kt[3] = {0.00389, 0.00825, 0.00389};
+ float ke[3] = {0.00389, 0.00825, 0.00389};
+
for (int i = 0; i < 3; i++)
pwm[i] = (resistance[i] * (torque[i] / kt[i]) + encoder.velocity[i]*ratio[i] * ke[i]) / voltage;
pidHand.compute();
motor[0].drive(-pwm[0]); //※これだけマイナスにするしかなかった
- for (int i = 1; i < 3; i++)
- motor[i].drive(pwm[i]);
+ motor[1].drive( pwm[1]);
+ motor[2].drive( pwm[2]);
motor[3].drive(pidHand.output);
-
- passtime += delta_t;
+ }
+ else {
+ can.setI(SERVO_NODE, 1, 20);
+ can.send();
}
can.setF(ARM_NODE, 1, imu.angle[2]);
can.send();
- /*can.setI(INFO, 1, flag);
- can.setI(INFO, 2, convergence);
- can.send();*/
+ can.setF(INFO_ID, 0, (float)convergence);
+ can.setF(INFO_ID, 1, towel_param[0]);
+ can.send();
}
@@ -136,9 +497,9 @@
for (int i = 0; i < 3; i++) {
pidPosition[i].sensor = &kinematics.x[i];
- pidPosition[i].target = &targetX[i].f;
+ pidPosition[i].target = &targetT[i].f;
pidVelocity[i].sensor = &kinematics.dx[i];
- pidVelocity[i].target = &targetX[i].df;
+ pidVelocity[i].target = &targetT[i].df;
}
pidHand.sensor = &encoder.angle[3];
pidHand.target = &kinematics.q[3];
@@ -147,16 +508,22 @@
while(1) {
//nh.spinOnce();
- if (start == 0) flag = true;
+ if (start == 0) convergence = 22;
+
pc.printf("x : %.3f ", kinematics.x[0]);
pc.printf("%.3f ", kinematics.x[1]);
pc.printf("%.3f\t", kinematics.x[2]);
- pc.printf("target : %.3f ", targetX[0].f);
- pc.printf("%.3f ", targetX[1].f);
- pc.printf("%.3f\t", targetX[2].f);
- pc.printf("pwm : %.3f ", targetX2[0].f);
- pc.printf("%.3f ", targetX2[1].f);
- pc.printf("%.3f\n", targetX2[2].f);
+ pc.printf("target : %.3f ", *pidPosition[0].target);
+ pc.printf("%.3f ", *pidPosition[1].target);
+ pc.printf("%.3f ", *pidPosition[2].target);
+ pc.printf("%.3f ", -pwm[0]);
+ pc.printf("%.3f ", pwm[1]);
+ pc.printf("%.3f\t", pwm[2]);
+ pc.printf("%.3f\t", encoder.angle[0]);
+ pc.printf("%d ", convergence);
+ pc.printf("%d ", arm_flag);
+ pc.printf("%d ", enc_num);
+ pc.printf("enc_error : %d\n", enc_error);
}