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@1:0839d3bc46c2, 2019-08-30 (annotated)

Committer:
Kirua
Date:
Fri Aug 30 03:07:17 2019 +0000
Revision:
1:0839d3bc46c2
Parent:
0:8b0869cb7fa8
Child:
2:9ada9cccb18b
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 0:8b0869cb7fa8 6 int size = msg.data_length;
Kirua 0:8b0869cb7fa8 7 for (int i = 0; i < size; i++)
Kirua 0:8b0869cb7fa8 8 coordinate[i] = msg.data[i];
Kirua 0:8b0869cb7fa8 9 }
Kirua 1:0839d3bc46c2 10 ros::Subscriber<std_msgs::Float32MultiArray> sub("bath_towel", callback);
Kirua 1:0839d3bc46c2 11 */
Kirua 0:8b0869cb7fa8 12
Kirua 1:0839d3bc46c2 13 void gurdTorque()
Kirua 1:0839d3bc46c2 14 {
Kirua 1:0839d3bc46c2 15 for(int i = 0; i < 3; i++) {
Kirua 1:0839d3bc46c2 16 if(abs(torque[i]) > max_torque[i]) {
Kirua 1:0839d3bc46c2 17 if(torque[i] > 0) torque[i] = max_torque[i];
Kirua 1:0839d3bc46c2 18 else torque[i] = -max_torque[i];
Kirua 1:0839d3bc46c2 19 }
Kirua 1:0839d3bc46c2 20 }
Kirua 1:0839d3bc46c2 21 }
Kirua 1:0839d3bc46c2 22
Kirua 1:0839d3bc46c2 23 void rescalePwm(float max)
Kirua 1:0839d3bc46c2 24 {
Kirua 1:0839d3bc46c2 25 float max_value = max;
Kirua 1:0839d3bc46c2 26
Kirua 1:0839d3bc46c2 27 for(int i = 0; i < 3; i++)
Kirua 1:0839d3bc46c2 28 if(abs(pwm[i]) > max_value)
Kirua 1:0839d3bc46c2 29 max_value = abs(pwm[i]);
Kirua 1:0839d3bc46c2 30 if(max_value > max)
Kirua 1:0839d3bc46c2 31 for(int i = 0; i < 3; i++)
Kirua 1:0839d3bc46c2 32 pwm[i] = pwm[i] / max_value * max;
Kirua 1:0839d3bc46c2 33 }
Kirua 0:8b0869cb7fa8 34
Kirua 0:8b0869cb7fa8 35
Kirua 1:0839d3bc46c2 36 void looping()
Kirua 1:0839d3bc46c2 37 {
Kirua 1:0839d3bc46c2 38 imu.computeAngles();
Kirua 1:0839d3bc46c2 39 /*for (int i = 0; i < 4; i++)
Kirua 1:0839d3bc46c2 40 encoder.getPosition(i);*/
Kirua 1:0839d3bc46c2 41 can.setF(ARM_NODE, 1, imu.angle[2]);
Kirua 1:0839d3bc46c2 42 can.send();
Kirua 1:0839d3bc46c2 43 kinematics.coordination();
Kirua 1:0839d3bc46c2 44 for (int i = 0; i < 3; i++)
Kirua 1:0839d3bc46c2 45 pidPosition[i].compute();
Kirua 1:0839d3bc46c2 46 kinematics.coordination();
Kirua 1:0839d3bc46c2 47 for (int i = 0; i < 3; i++)
Kirua 1:0839d3bc46c2 48 pidVelocity[i].compute();
Kirua 1:0839d3bc46c2 49 pidHand.compute();
Kirua 1:0839d3bc46c2 50 inertia.calculate();
Kirua 1:0839d3bc46c2 51 if(convergence[0] && convergence[1] && convergence[2])
Kirua 1:0839d3bc46c2 52 for (int i = 0; i < 3; i++)
Kirua 1:0839d3bc46c2 53 torque[i] = inertia.gravity[i];
Kirua 1:0839d3bc46c2 54 else
Kirua 1:0839d3bc46c2 55 for (int i = 0; i < 3; i++)
Kirua 1:0839d3bc46c2 56 torque[i] = pidVelocity[i].output * inertia.i_moment[i] + inertia.gravity[i];
Kirua 1:0839d3bc46c2 57 gurdTorque();
Kirua 1:0839d3bc46c2 58 for (int i = 0; i < 3; i++)
Kirua 1:0839d3bc46c2 59 pwm[i] = (resistance[i] * (torque[i] / kt[i]) + encoder.velocity[i]*ratio[i] * ke[i]) / voltage;
Kirua 1:0839d3bc46c2 60 rescalePwm(0.95);
Kirua 1:0839d3bc46c2 61 for (int i = 0; i < 3; i++)
Kirua 1:0839d3bc46c2 62 motor[i].drive(pwm[i]);
Kirua 1:0839d3bc46c2 63 motor[3].drive(pidHand.output);
Kirua 1:0839d3bc46c2 64 for (int i = 0; i < 3; i++)
Kirua 1:0839d3bc46c2 65 convergence[i] = pidPosition[i].isConvergence();
Kirua 1:0839d3bc46c2 66 }
Kirua 1:0839d3bc46c2 67
Kirua 1:0839d3bc46c2 68
Kirua 1:0839d3bc46c2 69 int main()
Kirua 1:0839d3bc46c2 70 {
Kirua 1:0839d3bc46c2 71 //nh.initNode();
Kirua 1:0839d3bc46c2 72 //nh.subscribe(sub);
Kirua 1:0839d3bc46c2 73 float offset_mag[3] = {24.375f, 12.6f, 0.0f};
Kirua 1:0839d3bc46c2 74 imu.setOffset(offset_mag);
Kirua 1:0839d3bc46c2 75 imu.performCalibration();
Kirua 1:0839d3bc46c2 76 encoder.inverse(0);
Kirua 1:0839d3bc46c2 77 for (int i = 0; i < 3; i++) {
Kirua 1:0839d3bc46c2 78 pidPosition[i].sensor = &kinematics.x[i];
Kirua 1:0839d3bc46c2 79 pidPosition[i].target = &target_coordinate[i];
Kirua 1:0839d3bc46c2 80 pidVelocity[i].sensor = &encoder.velocity[i];
Kirua 1:0839d3bc46c2 81 pidVelocity[i].target = &kinematics.vector_q[i];
Kirua 1:0839d3bc46c2 82 }
Kirua 1:0839d3bc46c2 83 pidHand.sensor = &encoder.angle[3];
Kirua 1:0839d3bc46c2 84 pidHand.target = &kinematics.q4;
Kirua 1:0839d3bc46c2 85 ticker.attach(&looping, delta_t);
Kirua 1:0839d3bc46c2 86
Kirua 0:8b0869cb7fa8 87 while(1) {
Kirua 1:0839d3bc46c2 88 //nh.spinOnce();
Kirua 1:0839d3bc46c2 89 pc.printf("pid : %.3f ", pidPosition[0].output);
Kirua 1:0839d3bc46c2 90 pc.printf("%.3f ", pidPosition[0].output);
Kirua 1:0839d3bc46c2 91 pc.printf("%.3f\t", pidPosition[0].output);
Kirua 1:0839d3bc46c2 92 pc.printf("pwm : %.3f ", pwm[0]);
Kirua 1:0839d3bc46c2 93 pc.printf("%.3f ", pwm[1]);
Kirua 1:0839d3bc46c2 94 pc.printf("%.3f\t", pwm[2]);
Kirua 1:0839d3bc46c2 95 pc.printf("enc : %.3f ", encoder.angle[0]);
Kirua 1:0839d3bc46c2 96 pc.printf("%.3f ", encoder.angle[1]);
Kirua 1:0839d3bc46c2 97 pc.printf("%.3f ", encoder.angle[2]);
Kirua 1:0839d3bc46c2 98 pc.printf("%.3f\t", encoder.angle[3]);
Kirua 1:0839d3bc46c2 99 pc.printf("imu : %.3f\n", imu.angle[2]);
Kirua 0:8b0869cb7fa8 100 }
Kirua 1:0839d3bc46c2 101
Kirua 1:0839d3bc46c2 102
Kirua 1:0839d3bc46c2 103 /* encoder_calibration */
Kirua 1:0839d3bc46c2 104 /*
Kirua 1:0839d3bc46c2 105 while(!encoder.setZero(0))
Kirua 1:0839d3bc46c2 106 pc.printf("No\n");
Kirua 1:0839d3bc46c2 107 pc.printf("yes we can!");
Kirua 1:0839d3bc46c2 108 */
Kirua 0:8b0869cb7fa8 109 }