File content as of revision 3:e6d0abef5936:

#include "settings.h"


/*
void callback(const std_msgs::Float32MultiArray& msg) {
    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(torque[i] > 0) torque[i] = max_torque[i];
            else torque[i] = -max_torque[i];
        }
    }
}

void looping()
{
    imu.computeAngles();
    
    for (int i = 0; i < 4; i++)
        encoder.getPosition(i);
    
    kinematics.computeKinematics();
    
    /*can.readI();
    convergence = can.get(INFO, 2);*/
    
    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;
    }
    
    can.setF(ARM_NODE, 1, imu.angle[2]);
    can.send();
    /*can.setI(INFO, 1, flag);
    can.setI(INFO, 2, convergence);
    can.send();*/
}


int main()
{
    //nh.initNode();
    //nh.subscribe(sub);
    
    float offset[9] = {0, 0, 0, 0, 0, 0, 24.375f, 12.6f, 0.0f};
    imu.setOffset(offset);
    imu.performCalibration();
    
    encoder.inverse(0);
    
    for (int i = 0; i < 3; i++) {
        pidPosition[i].sensor = &kinematics.x[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.q[3];
    
    ticker.attach(&looping, delta_t);
    
    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 ", targetX2[0].f);
        pc.printf("%.3f ", targetX2[1].f);
        pc.printf("%.3f\n", targetX2[2].f);
    }
    
    
    /* encoder_calibration */
    /*
    while(!encoder.setZero(0))
            pc.printf("No\n");
    pc.printf("yes we can!");
    */
}