Diff: main.cpp

Revision:

2:9ada9cccb18b

Parent:

1:0839d3bc46c2

Child:

3:e6d0abef5936

--- a/main.cpp	Fri Aug 30 03:07:17 2019 +0000
+++ b/main.cpp	Sun Sep 15 17:41:07 2019 +0000
@@ -3,66 +3,69 @@
 
 /*
 void callback(const std_msgs::Float32MultiArray& msg) {
-    int size = msg.data_length;
-    for (int i = 0; i < size; i++)
-        coordinate[i] = msg.data[i];
+    for (int i = 0; i < msg.data_length; i++)
+        towel_param[i] = msg.data[i];
 }
 ros::Subscriber<std_msgs::Float32MultiArray> sub("bath_towel", callback);
 */
-
 void gurdTorque()
 {
     for(int i = 0; i < 3; i++) {
-        if(abs(torque[i]) > max_torque[i]) { 
+        if(abs(torque[i]) > max_torque[i]) {
             if(torque[i] > 0) torque[i] = max_torque[i];
             else torque[i] = -max_torque[i];
         }
     }
 }
 
-void rescalePwm(float max)
+void canFunc()
 {
-    float max_value = max;
-    
-    for(int i = 0; i < 3; i++)
-        if(abs(pwm[i]) > max_value)
-            max_value = abs(pwm[i]);
-    if(max_value > max)
-        for(int i = 0; i < 3; i++)
-            pwm[i] = pwm[i] / max_value * max;
+    can.setF(ARM_NODE, 1, imu.angle[2]);
+    can.send();
 }
 
 
 void looping()
 {
     imu.computeAngles();
-    /*for (int i = 0; i < 4; i++)
-        encoder.getPosition(i);*/
-    can.setF(ARM_NODE, 1, imu.angle[2]);
-    can.send();
-    kinematics.coordination();
-    for (int i = 0; i < 3; i++)
-        pidPosition[i].compute();
-    kinematics.coordination();
+    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();
     for (int i = 0; i < 3; i++)
-        pidVelocity[i].compute();
-    pidHand.compute();
+        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[0] && convergence[1] && convergence[2])
-        for (int i = 0; i < 3; i++)
-            torque[i] = inertia.gravity[i];
-    else 
-        for (int i = 0; i < 3; i++)
-            torque[i] = pidVelocity[i].output * inertia.i_moment[i] + inertia.gravity[i];
+    
+    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;
-    rescalePwm(0.95);
-    for (int i = 0; i < 3; i++)
+    
+    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);
-    for (int i = 0; i < 3; i++)
-        convergence[i] = pidPosition[i].isConvergence();
+    
+    passtime += delta_t;
 }
 
 
@@ -70,39 +73,41 @@
 {
     //nh.initNode();
     //nh.subscribe(sub);
+    
     float offset_mag[3] = {24.375f, 12.6f, 0.0f};
     imu.setOffset(offset_mag);
     imu.performCalibration();
+    
     encoder.inverse(0);
+    
     for (int i = 0; i < 3; i++) {
         pidPosition[i].sensor = &kinematics.x[i];
-        pidPosition[i].target = &target_coordinate[i];
-        pidVelocity[i].sensor = &encoder.velocity[i];
-        pidVelocity[i].target = &kinematics.vector_q[i];
+        pidPosition[i].target = &targetX[i].f;
+        pidVelocity[i].sensor = &kinematics.dx[i];
+        pidVelocity[i].target = &targetX[i].df;
     }
     pidHand.sensor = &encoder.angle[3];
-    pidHand.target = &kinematics.q4;
+    pidHand.target = &kinematics.q[3];
+    
     ticker.attach(&looping, delta_t);
     
     while(1) {
         //nh.spinOnce();
-        pc.printf("pid : %.3f ", pidPosition[0].output);
-        pc.printf("%.3f ", pidPosition[0].output);
-        pc.printf("%.3f\t", pidPosition[0].output);
-        pc.printf("pwm : %.3f ", pwm[0]);
+        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\t", pwm[2]);
-        pc.printf("enc : %.3f ", encoder.angle[0]);
-        pc.printf("%.3f ", encoder.angle[1]);
-        pc.printf("%.3f ", encoder.angle[2]);
-        pc.printf("%.3f\t", encoder.angle[3]);
-        pc.printf("imu : %.3f\n", imu.angle[2]);
+        pc.printf("%.3f\n", pwm[2]);
     }
     
     
     /* encoder_calibration */
     /*
-    while(!encoder.setZero(0))
+    while(!encoder.setZero(2))
             pc.printf("No\n");
     pc.printf("yes we can!");
     */