Diff: main.cpp

Revision:

1:0839d3bc46c2

Parent:

0:8b0869cb7fa8

Child:

2:9ada9cccb18b

--- a/main.cpp	Mon Jul 22 17:49:09 2019 +0000
+++ b/main.cpp	Fri Aug 30 03:07:17 2019 +0000
@@ -1,38 +1,109 @@
-#include "mbed.h"
-#include <ros.h>
-#include <std_msgs/Float32MultiArray.h>
-#include "set.h"
-
-#define inertia M*R*R //moment of inertia
-
-float voltage;
-float resistance;
-float kt;
-float ke;
-float *w;
-float torque;
-float pwm = 1 / voltage * ((resistance * (torque) / kt) + (*w * ke));
-
-ros::NodeHandle nh;
-DigitalOut myled(LED1);
-
-float coordinate[3] = {};
+#include "settings.h"
 
 
+/*
 void callback(const std_msgs::Float32MultiArray& msg) {
     int size = msg.data_length;
     for (int i = 0; i < size; i++)
         coordinate[i] = msg.data[i];
 }
+ros::Subscriber<std_msgs::Float32MultiArray> sub("bath_towel", callback);
+*/
 
-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 rescalePwm(float max)
+{
+    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;
+}
 
 
-int main() {
-    nh.initNode();
-    nh.subscribe(sub);
+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 < 3; i++)
+        pidVelocity[i].compute();
+    pidHand.compute();
+    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];
+    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++)
+        motor[i].drive(pwm[i]);
+    motor[3].drive(pidHand.output);
+    for (int i = 0; i < 3; i++)
+        convergence[i] = pidPosition[i].isConvergence();
+}
+
+
+int main()
+{
+    //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];
+    }
+    pidHand.sensor = &encoder.angle[3];
+    pidHand.target = &kinematics.q4;
+    ticker.attach(&looping, delta_t);
+    
     while(1) {
-        nh.spinOnce();
-        wait_ms(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("%.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]);
     }
+    
+    
+    /* encoder_calibration */
+    /*
+    while(!encoder.setZero(0))
+            pc.printf("No\n");
+    pc.printf("yes we can!");
+    */
 }