Diff: main.cpp

Revision:

4:7c19daf24a7d

Parent:

3:e6d0abef5936

Child:

5:a6d22b209980

--- 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);
     }