ichinoseki_Bteam_2019 / Mbed 2 deprecated 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

main.cpp@3:e6d0abef5936, 2019-09-19 (annotated)

Committer:
Kirua
Date:
Thu Sep 19 13:01:46 2019 +0000
Revision:
3:e6d0abef5936
Parent:
2:9ada9cccb18b
Child:
4:7c19daf24a7d
fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kirua 1:0839d3bc46c2 1 #include "settings.h"
Kirua 0:8b0869cb7fa8 2
Kirua 0:8b0869cb7fa8 3
Kirua 1:0839d3bc46c2 4 /*
Kirua 0:8b0869cb7fa8 5 void callback(const std_msgs::Float32MultiArray& msg) {
Kirua 2:9ada9cccb18b 6 for (int i = 0; i < msg.data_length; i++)
Kirua 2:9ada9cccb18b 7 towel_param[i] = msg.data[i];
Kirua 0:8b0869cb7fa8 8 }
Kirua 1:0839d3bc46c2 9 ros::Subscriber<std_msgs::Float32MultiArray> sub("bath_towel", callback);
Kirua 1:0839d3bc46c2 10 */
Kirua 1:0839d3bc46c2 11 void gurdTorque()
Kirua 1:0839d3bc46c2 12 {
Kirua 1:0839d3bc46c2 13 for(int i = 0; i < 3; i++) {
Kirua 2:9ada9cccb18b 14 if(abs(torque[i]) > max_torque[i]) {
Kirua 1:0839d3bc46c2 15 if(torque[i] > 0) torque[i] = max_torque[i];
Kirua 1:0839d3bc46c2 16 else torque[i] = -max_torque[i];
Kirua 1:0839d3bc46c2 17 }
Kirua 1:0839d3bc46c2 18 }
Kirua 1:0839d3bc46c2 19 }
Kirua 1:0839d3bc46c2 20
Kirua 1:0839d3bc46c2 21 void looping()
Kirua 1:0839d3bc46c2 22 {
Kirua 1:0839d3bc46c2 23 imu.computeAngles();
Kirua 3:e6d0abef5936 24
Kirua 2:9ada9cccb18b 25 for (int i = 0; i < 4; i++)
Kirua 2:9ada9cccb18b 26 encoder.getPosition(i);
Kirua 2:9ada9cccb18b 27
Kirua 3:e6d0abef5936 28 kinematics.computeKinematics();
Kirua 2:9ada9cccb18b 29
Kirua 3:e6d0abef5936 30 /*can.readI();
Kirua 3:e6d0abef5936 31 convergence = can.get(INFO, 2);*/
Kirua 2:9ada9cccb18b 32
Kirua 3:e6d0abef5936 33 if (convergence >= 9)
Kirua 3:e6d0abef5936 34 flag = false;
Kirua 3:e6d0abef5936 35 if (flag)
Kirua 3:e6d0abef5936 36 {
Kirua 3:e6d0abef5936 37 /*
Kirua 3:e6d0abef5936 38 switch (convergence) {
Kirua 3:e6d0abef5936 39 case 3:
Kirua 3:e6d0abef5936 40 if (passtime <= total - 1)
Kirua 3:e6d0abef5936 41 for (int i = 0; i < 3; i++)
Kirua 3:e6d0abef5936 42 targetX[i].Calc(passtime);
Kirua 3:e6d0abef5936 43 break;
Kirua 3:e6d0abef5936 44 case 7:
Kirua 3:e6d0abef5936 45 if (passtime <= total - 1)
Kirua 3:e6d0abef5936 46 for (int i = 0; i < 3; i++)
Kirua 3:e6d0abef5936 47 targetX[i].Calc(passtime);
Kirua 3:e6d0abef5936 48 break;
Kirua 3:e6d0abef5936 49 }*/
Kirua 3:e6d0abef5936 50 if (passtime <= total - 1 && !inverse_flag) {
Kirua 3:e6d0abef5936 51 for (int i = 0; i < 3; i++)
Kirua 3:e6d0abef5936 52 targetX[i].Calc(passtime);
Kirua 3:e6d0abef5936 53 kinematics.HorizontalVertical(VERTICAL);
Kirua 3:e6d0abef5936 54 can.setI(SERVO_NODE, 1, 1);
Kirua 3:e6d0abef5936 55 can.send();
Kirua 3:e6d0abef5936 56 }
Kirua 3:e6d0abef5936 57 else if (passtime <= total - 1 && inverse_flag) {
Kirua 3:e6d0abef5936 58 for (int i = 0; i < 3; i++)
Kirua 3:e6d0abef5936 59 targetX2[i].Calc(passtime);
Kirua 3:e6d0abef5936 60 kinematics.HorizontalVertical(HORIZONTAL);
Kirua 3:e6d0abef5936 61 can.setI(SERVO_NODE, 1, 0);
Kirua 3:e6d0abef5936 62 can.send();
Kirua 3:e6d0abef5936 63 }
Kirua 3:e6d0abef5936 64 else if (position_convergence[0] && position_convergence[1] && position_convergence[2] && !inverse_flag) {
Kirua 3:e6d0abef5936 65 can.setI(SERVO_NODE, 1, 0);
Kirua 3:e6d0abef5936 66 can.send();
Kirua 3:e6d0abef5936 67 inverse_flag = true;
Kirua 3:e6d0abef5936 68 passtime = 0;
Kirua 3:e6d0abef5936 69 for (int i = 0; i < 3; i++) {
Kirua 3:e6d0abef5936 70 pidPosition[i].target = &targetX2[i].f;
Kirua 3:e6d0abef5936 71 pidVelocity[i].target = &targetX2[i].df;
Kirua 3:e6d0abef5936 72 }
Kirua 3:e6d0abef5936 73 }
Kirua 3:e6d0abef5936 74 else if (position_convergence[0] && position_convergence[1] && position_convergence[2] && inverse_flag) {
Kirua 3:e6d0abef5936 75 can.setI(SERVO_NODE, 1, 1);
Kirua 3:e6d0abef5936 76 can.send();
Kirua 3:e6d0abef5936 77 }
Kirua 3:e6d0abef5936 78
Kirua 3:e6d0abef5936 79 for (int i = 0; i < 3; i++)
Kirua 3:e6d0abef5936 80 position_convergence[i] = pidPosition[i].compute();
Kirua 3:e6d0abef5936 81 kinematics.computeJacobian();
Kirua 3:e6d0abef5936 82 for (int i = 0; i < 3; i++)
Kirua 3:e6d0abef5936 83 velocity_convergence[i] = pidVelocity[i].compute();
Kirua 3:e6d0abef5936 84 for (int i = 0; i < 3; i++)
Kirua 3:e6d0abef5936 85 a_result[i] = targetX[i].ddf + pidVelocity[i].output + pidPosition[i].output;
Kirua 3:e6d0abef5936 86
Kirua 3:e6d0abef5936 87 kinematics.computeAcceleration();
Kirua 3:e6d0abef5936 88
Kirua 3:e6d0abef5936 89 inertia.calculate();
Kirua 3:e6d0abef5936 90
Kirua 3:e6d0abef5936 91 if (convergence == 3) {
Kirua 3:e6d0abef5936 92 torque[0] = (inertia.gravity[0] + kinematics.ddq[0] * inertia.i_moment[0]) / (ratio[0] * 0.5);
Kirua 3:e6d0abef5936 93 torque[1] = (inertia.gravity[1] + kinematics.ddq[1] * inertia.i_moment[1]) / (ratio[1] * 0.6);
Kirua 3:e6d0abef5936 94 torque[2] = (inertia.gravity[2] + kinematics.ddq[2] * inertia.i_moment[2]) / (ratio[2] * 0.6);
Kirua 3:e6d0abef5936 95 }
Kirua 3:e6d0abef5936 96 else {
Kirua 3:e6d0abef5936 97 torque[0] = (inertia.gravity[0] + kinematics.ddq[0] * inertia.i_moment[0]) / (ratio[0] * 0.5);
Kirua 3:e6d0abef5936 98 torque[1] = (inertia.gravity[1] + kinematics.ddq[1] * inertia.i_moment[1]) / (ratio[1] * 0.6);
Kirua 3:e6d0abef5936 99 torque[2] = (inertia.gravity[2] + kinematics.ddq[2] * inertia.i_moment[2]) / (ratio[2] * 0.6);
Kirua 3:e6d0abef5936 100 /*torque[0] = inertia.gravity[0] / (ratio[0] * 0.5);
Kirua 3:e6d0abef5936 101 torque[1] = inertia.gravity[1] / (ratio[1] * 0.6);
Kirua 3:e6d0abef5936 102 torque[2] = inertia.gravity[2] / (ratio[2] * 0.6);*/
Kirua 3:e6d0abef5936 103 }
Kirua 3:e6d0abef5936 104 gurdTorque();
Kirua 3:e6d0abef5936 105 for (int i = 0; i < 3; i++)
Kirua 3:e6d0abef5936 106 pwm[i] = (resistance[i] * (torque[i] / kt[i]) + encoder.velocity[i]*ratio[i] * ke[i]) / voltage;
Kirua 3:e6d0abef5936 107
Kirua 3:e6d0abef5936 108 pidHand.compute();
Kirua 3:e6d0abef5936 109
Kirua 3:e6d0abef5936 110 motor[0].drive(-pwm[0]); //※これだけマイナスにするしかなかった
Kirua 3:e6d0abef5936 111 for (int i = 1; i < 3; i++)
Kirua 3:e6d0abef5936 112 motor[i].drive(pwm[i]);
Kirua 3:e6d0abef5936 113 motor[3].drive(pidHand.output);
Kirua 3:e6d0abef5936 114
Kirua 3:e6d0abef5936 115 passtime += delta_t;
Kirua 3:e6d0abef5936 116 }
Kirua 2:9ada9cccb18b 117
Kirua 3:e6d0abef5936 118 can.setF(ARM_NODE, 1, imu.angle[2]);
Kirua 3:e6d0abef5936 119 can.send();
Kirua 3:e6d0abef5936 120 /*can.setI(INFO, 1, flag);
Kirua 3:e6d0abef5936 121 can.setI(INFO, 2, convergence);
Kirua 3:e6d0abef5936 122 can.send();*/
Kirua 1:0839d3bc46c2 123 }
Kirua 1:0839d3bc46c2 124
Kirua 1:0839d3bc46c2 125
Kirua 1:0839d3bc46c2 126 int main()
Kirua 1:0839d3bc46c2 127 {
Kirua 1:0839d3bc46c2 128 //nh.initNode();
Kirua 1:0839d3bc46c2 129 //nh.subscribe(sub);
Kirua 2:9ada9cccb18b 130
Kirua 3:e6d0abef5936 131 float offset[9] = {0, 0, 0, 0, 0, 0, 24.375f, 12.6f, 0.0f};
Kirua 3:e6d0abef5936 132 imu.setOffset(offset);
Kirua 1:0839d3bc46c2 133 imu.performCalibration();
Kirua 2:9ada9cccb18b 134
Kirua 1:0839d3bc46c2 135 encoder.inverse(0);
Kirua 2:9ada9cccb18b 136
Kirua 1:0839d3bc46c2 137 for (int i = 0; i < 3; i++) {
Kirua 1:0839d3bc46c2 138 pidPosition[i].sensor = &kinematics.x[i];
Kirua 2:9ada9cccb18b 139 pidPosition[i].target = &targetX[i].f;
Kirua 2:9ada9cccb18b 140 pidVelocity[i].sensor = &kinematics.dx[i];
Kirua 2:9ada9cccb18b 141 pidVelocity[i].target = &targetX[i].df;
Kirua 1:0839d3bc46c2 142 }
Kirua 1:0839d3bc46c2 143 pidHand.sensor = &encoder.angle[3];
Kirua 2:9ada9cccb18b 144 pidHand.target = &kinematics.q[3];
Kirua 2:9ada9cccb18b 145
Kirua 1:0839d3bc46c2 146 ticker.attach(&looping, delta_t);
Kirua 1:0839d3bc46c2 147
Kirua 0:8b0869cb7fa8 148 while(1) {
Kirua 1:0839d3bc46c2 149 //nh.spinOnce();
Kirua 3:e6d0abef5936 150 if (start == 0) flag = true;
Kirua 2:9ada9cccb18b 151 pc.printf("x : %.3f ", kinematics.x[0]);
Kirua 2:9ada9cccb18b 152 pc.printf("%.3f ", kinematics.x[1]);
Kirua 2:9ada9cccb18b 153 pc.printf("%.3f\t", kinematics.x[2]);
Kirua 2:9ada9cccb18b 154 pc.printf("target : %.3f ", targetX[0].f);
Kirua 2:9ada9cccb18b 155 pc.printf("%.3f ", targetX[1].f);
Kirua 2:9ada9cccb18b 156 pc.printf("%.3f\t", targetX[2].f);
Kirua 3:e6d0abef5936 157 pc.printf("pwm : %.3f ", targetX2[0].f);
Kirua 3:e6d0abef5936 158 pc.printf("%.3f ", targetX2[1].f);
Kirua 3:e6d0abef5936 159 pc.printf("%.3f\n", targetX2[2].f);
Kirua 0:8b0869cb7fa8 160 }
Kirua 1:0839d3bc46c2 161
Kirua 1:0839d3bc46c2 162
Kirua 1:0839d3bc46c2 163 /* encoder_calibration */
Kirua 1:0839d3bc46c2 164 /*
Kirua 3:e6d0abef5936 165 while(!encoder.setZero(0))
Kirua 1:0839d3bc46c2 166 pc.printf("No\n");
Kirua 1:0839d3bc46c2 167 pc.printf("yes we can!");
Kirua 1:0839d3bc46c2 168 */
Kirua 0:8b0869cb7fa8 169 }