Diff: main.cpp

Revision:

3:e6d0abef5936

Parent:

2:9ada9cccb18b

Child:

4:7c19daf24a7d

--- a/main.cpp	Sun Sep 15 17:41:07 2019 +0000
+++ b/main.cpp	Thu Sep 19 13:01:46 2019 +0000
@@ -18,54 +18,108 @@
     }
 }
 
-void canFunc()
-{
-    can.setF(ARM_NODE, 1, imu.angle[2]);
-    can.send();
-}
-
-
 void looping()
 {
     imu.computeAngles();
+    
     for (int i = 0; i < 4; i++)
         encoder.getPosition(i);
     
-    canFunc();
-    
-    if (passtime <= total - 1)
-        for (int i = 0; i < 3; i++)
-            targetX[i].Calc(passtime);
+    kinematics.computeKinematics();
     
-    kinematics.computeKinematics();
-    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();
+    /*can.readI();
+    convergence = can.get(INFO, 2);*/
     
-    inertia.calculate();
-    
-    for (int i = 0; i < 3; i++)
-        torque[i] = inertia.gravity[i] + kinematics.ddq[i] * inertia.i_moment[i];
-    
-    gurdTorque();
-    for (int i = 0; i < 3; i++)
-        pwm[i] = (resistance[i] * (torque[i] / kt[i]) + encoder.velocity[i]*ratio[i] * ke[i]) / voltage;
+    if (convergence >= 9)
+        flag = false;
+    if (flag)
+    {
+        /*
+        switch (convergence) {
+            case 3:
+                if (passtime <= total - 1)
+                    for (int i = 0; i < 3; i++)
+                        targetX[i].Calc(passtime);
+                break;
+            case 7:
+                if (passtime <= total - 1)
+                    for (int i = 0; i < 3; i++)
+                        targetX[i].Calc(passtime);
+                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();
+        }
+        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();
+        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();
+        
+        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);
+        }
+        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();
+        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[3].drive(pidHand.output);
+        
+        passtime += delta_t;
+    }
     
-    kinematics.HorizontalVertical(VERTICAL);
-    pidHand.compute();
-    
-    motor[0].drive(-pwm[0]);    //※これだけマイナスにするしかなかった
-    for (int i = 1; i < 3; i++)
-        motor[i].drive(pwm[i]);
-    motor[3].drive(pidHand.output);
-    
-    passtime += delta_t;
+    can.setF(ARM_NODE, 1, imu.angle[2]);
+    can.send();
+    /*can.setI(INFO, 1, flag);
+    can.setI(INFO, 2, convergence);
+    can.send();*/
 }
 
 
@@ -74,8 +128,8 @@
     //nh.initNode();
     //nh.subscribe(sub);
     
-    float offset_mag[3] = {24.375f, 12.6f, 0.0f};
-    imu.setOffset(offset_mag);
+    float offset[9] = {0, 0, 0, 0, 0, 0, 24.375f, 12.6f, 0.0f};
+    imu.setOffset(offset);
     imu.performCalibration();
     
     encoder.inverse(0);
@@ -93,21 +147,22 @@
     
     while(1) {
         //nh.spinOnce();
+        if (start == 0) flag = true;
         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 ", -pwm[0]);
-        pc.printf("%.3f ", pwm[1]);
-        pc.printf("%.3f\n", pwm[2]);
+        pc.printf("pwm : %.3f ", targetX2[0].f);
+        pc.printf("%.3f ", targetX2[1].f);
+        pc.printf("%.3f\n", targetX2[2].f);
     }
     
     
     /* encoder_calibration */
     /*
-    while(!encoder.setZero(2))
+    while(!encoder.setZero(0))
             pc.printf("No\n");
     pc.printf("yes we can!");
     */