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@2:9ada9cccb18b, 2019-09-15 (annotated)

Committer:
Kirua
Date:
Sun Sep 15 17:41:07 2019 +0000
Revision:
2:9ada9cccb18b
Parent:
1:0839d3bc46c2
Child:
3:e6d0abef5936
new

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 2:9ada9cccb18b 21 void canFunc()
Kirua 1:0839d3bc46c2 22 {
Kirua 2:9ada9cccb18b 23 can.setF(ARM_NODE, 1, imu.angle[2]);
Kirua 2:9ada9cccb18b 24 can.send();
Kirua 1:0839d3bc46c2 25 }
Kirua 0:8b0869cb7fa8 26
Kirua 0:8b0869cb7fa8 27
Kirua 1:0839d3bc46c2 28 void looping()
Kirua 1:0839d3bc46c2 29 {
Kirua 1:0839d3bc46c2 30 imu.computeAngles();
Kirua 2:9ada9cccb18b 31 for (int i = 0; i < 4; i++)
Kirua 2:9ada9cccb18b 32 encoder.getPosition(i);
Kirua 2:9ada9cccb18b 33
Kirua 2:9ada9cccb18b 34 canFunc();
Kirua 2:9ada9cccb18b 35
Kirua 2:9ada9cccb18b 36 if (passtime <= total - 1)
Kirua 2:9ada9cccb18b 37 for (int i = 0; i < 3; i++)
Kirua 2:9ada9cccb18b 38 targetX[i].Calc(passtime);
Kirua 2:9ada9cccb18b 39
Kirua 2:9ada9cccb18b 40 kinematics.computeKinematics();
Kirua 1:0839d3bc46c2 41 for (int i = 0; i < 3; i++)
Kirua 2:9ada9cccb18b 42 position_convergence[i] = pidPosition[i].compute();
Kirua 2:9ada9cccb18b 43 kinematics.computeJacobian();
Kirua 2:9ada9cccb18b 44 for (int i = 0; i < 3; i++)
Kirua 2:9ada9cccb18b 45 velocity_convergence[i] = pidVelocity[i].compute();
Kirua 2:9ada9cccb18b 46 for (int i = 0; i < 3; i++)
Kirua 2:9ada9cccb18b 47 a_result[i] = targetX[i].ddf + pidVelocity[i].output + pidPosition[i].output;
Kirua 2:9ada9cccb18b 48
Kirua 2:9ada9cccb18b 49 kinematics.computeAcceleration();
Kirua 2:9ada9cccb18b 50
Kirua 1:0839d3bc46c2 51 inertia.calculate();
Kirua 2:9ada9cccb18b 52
Kirua 2:9ada9cccb18b 53 for (int i = 0; i < 3; i++)
Kirua 2:9ada9cccb18b 54 torque[i] = inertia.gravity[i] + kinematics.ddq[i] * inertia.i_moment[i];
Kirua 2:9ada9cccb18b 55
Kirua 1:0839d3bc46c2 56 gurdTorque();
Kirua 1:0839d3bc46c2 57 for (int i = 0; i < 3; i++)
Kirua 1:0839d3bc46c2 58 pwm[i] = (resistance[i] * (torque[i] / kt[i]) + encoder.velocity[i]*ratio[i] * ke[i]) / voltage;
Kirua 2:9ada9cccb18b 59
Kirua 2:9ada9cccb18b 60 kinematics.HorizontalVertical(VERTICAL);
Kirua 2:9ada9cccb18b 61 pidHand.compute();
Kirua 2:9ada9cccb18b 62
Kirua 2:9ada9cccb18b 63 motor[0].drive(-pwm[0]); //※これだけマイナスにするしかなかった
Kirua 2:9ada9cccb18b 64 for (int i = 1; i < 3; i++)
Kirua 1:0839d3bc46c2 65 motor[i].drive(pwm[i]);
Kirua 1:0839d3bc46c2 66 motor[3].drive(pidHand.output);
Kirua 2:9ada9cccb18b 67
Kirua 2:9ada9cccb18b 68 passtime += delta_t;
Kirua 1:0839d3bc46c2 69 }
Kirua 1:0839d3bc46c2 70
Kirua 1:0839d3bc46c2 71
Kirua 1:0839d3bc46c2 72 int main()
Kirua 1:0839d3bc46c2 73 {
Kirua 1:0839d3bc46c2 74 //nh.initNode();
Kirua 1:0839d3bc46c2 75 //nh.subscribe(sub);
Kirua 2:9ada9cccb18b 76
Kirua 1:0839d3bc46c2 77 float offset_mag[3] = {24.375f, 12.6f, 0.0f};
Kirua 1:0839d3bc46c2 78 imu.setOffset(offset_mag);
Kirua 1:0839d3bc46c2 79 imu.performCalibration();
Kirua 2:9ada9cccb18b 80
Kirua 1:0839d3bc46c2 81 encoder.inverse(0);
Kirua 2:9ada9cccb18b 82
Kirua 1:0839d3bc46c2 83 for (int i = 0; i < 3; i++) {
Kirua 1:0839d3bc46c2 84 pidPosition[i].sensor = &kinematics.x[i];
Kirua 2:9ada9cccb18b 85 pidPosition[i].target = &targetX[i].f;
Kirua 2:9ada9cccb18b 86 pidVelocity[i].sensor = &kinematics.dx[i];
Kirua 2:9ada9cccb18b 87 pidVelocity[i].target = &targetX[i].df;
Kirua 1:0839d3bc46c2 88 }
Kirua 1:0839d3bc46c2 89 pidHand.sensor = &encoder.angle[3];
Kirua 2:9ada9cccb18b 90 pidHand.target = &kinematics.q[3];
Kirua 2:9ada9cccb18b 91
Kirua 1:0839d3bc46c2 92 ticker.attach(&looping, delta_t);
Kirua 1:0839d3bc46c2 93
Kirua 0:8b0869cb7fa8 94 while(1) {
Kirua 1:0839d3bc46c2 95 //nh.spinOnce();
Kirua 2:9ada9cccb18b 96 pc.printf("x : %.3f ", kinematics.x[0]);
Kirua 2:9ada9cccb18b 97 pc.printf("%.3f ", kinematics.x[1]);
Kirua 2:9ada9cccb18b 98 pc.printf("%.3f\t", kinematics.x[2]);
Kirua 2:9ada9cccb18b 99 pc.printf("target : %.3f ", targetX[0].f);
Kirua 2:9ada9cccb18b 100 pc.printf("%.3f ", targetX[1].f);
Kirua 2:9ada9cccb18b 101 pc.printf("%.3f\t", targetX[2].f);
Kirua 2:9ada9cccb18b 102 pc.printf("pwm : %.3f ", -pwm[0]);
Kirua 1:0839d3bc46c2 103 pc.printf("%.3f ", pwm[1]);
Kirua 2:9ada9cccb18b 104 pc.printf("%.3f\n", pwm[2]);
Kirua 0:8b0869cb7fa8 105 }
Kirua 1:0839d3bc46c2 106
Kirua 1:0839d3bc46c2 107
Kirua 1:0839d3bc46c2 108 /* encoder_calibration */
Kirua 1:0839d3bc46c2 109 /*
Kirua 2:9ada9cccb18b 110 while(!encoder.setZero(2))
Kirua 1:0839d3bc46c2 111 pc.printf("No\n");
Kirua 1:0839d3bc46c2 112 pc.printf("yes we can!");
Kirua 1:0839d3bc46c2 113 */
Kirua 0:8b0869cb7fa8 114 }