Diff: main.cpp

Revision:

0:bf3a2eb5c7df

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Mar 22 11:29:35 2020 +0000
@@ -0,0 +1,90 @@
+// Nucleo_CAN_slave (AIRo-4.1)
+// Angle control
+// Created by Atsushi Kakogawa, 2019.09.19
+// Edited by Yoshimichi Oka, 2020.2.5
+// Modified by Atsushi Kakogawa, 2020.03.22
+// Department of Robotics, Ritsumeikan University, Japan
+#include "mbed.h"
+#include "CAN.h"
+
+Ticker control; // timer for control
+
+DigitalOut myled(PF_1);     // LED for communication check
+DigitalOut mdir1(PA_9);     // D1 Motor ON/OFF (HIGH = ON, Changeable on ESCON Studio)
+DigitalOut mdir2(PA_10);    // D0 Rotational direction (Direction is changeable on ESCON Studio)
+AnalogOut DA_crt(PA_4);     // A3 for current anaglog input to ESCON ANI1+ (0 to 1.0)
+AnalogOut DA_vlc(PA_6);     // A5 for velocity analog input to ESCON ANI2+ (0 to 1.0)
+AnalogIn potensio1(PA_0);   // A0 Potentiometer 1 (3.3V potentiometer)
+//AnalogIn potensio2(PA_0); // A0 Potentiometer 2 (5V potentiometer)
+AnalogIn AD_crt(PA_1);      // Analog Signal from ESCON ANO1 (motor velocity, can be changed by ESCON) (0 to 1.0)
+AnalogIn AD_vlc(PA_3);      // Analog Signal from ESCON ANO2 (motor current,  can be changed by ESCON) (0 to 1.0)
+
+float duty = 0;
+int id = 2;
+int flag = 0;
+float target_ang1 = 165, ang, e = 0, pang, de = 0;
+float kp=0.01, kd = 0.001;
+
+char tx_data[8];
+char tx_data1_U, tx_data1_L, tx_data2_U, tx_data2_L, tx_data3_U, tx_data3_L;
+
+// PD-control timer
+void controller() {
+    ang = potensio1.read()*360;
+    e = target_ang1 - ang;
+    de = ang - pang;
+    DA_crt = kp*abs(e) - kd*abs(de);
+    if (e > 0) {
+        mdir1 = 1;
+        mdir2 = 0;
+    } else if (e < 0) {
+        mdir1 = 1;
+        mdir2 = 1;
+    } else {
+        mdir1 = 0;
+        mdir2 = 0;
+    } 
+    pang = ang;
+}
+
+int main() {
+    
+    control.attach(&controller, 0.001);  // 1 ms
+
+    CAN can(PA_11, PA_12);
+    can.frequency(1000000);
+    CANMessage msg;
+   
+    while(1) {
+        if(can.read(msg)) {
+            if (msg.data[0] == id) {                                    // ID indentify
+                if (msg.data[1] == 0) {                                 // mode indentify (0: control)
+                    target_ang1 = (msg.data[3] << 8) + msg.data[4];
+                } else if (msg.data[1] == 1) {                          // mode indentify (1: response)
+                    int i_data1 = AD_crt.read()*1000; 
+                        tx_data1_U = (i_data1 >> 8) & 0xff;
+                        tx_data1_L = i_data1 & 0xff;
+                    int i_data2 = AD_vlc.read()*1000; 
+                        tx_data2_U = (i_data2 >> 8) & 0xff;
+                        tx_data2_L = i_data2 & 0xff;
+                    int i_data3 = potensio1.read()*360*100; 
+                        tx_data3_U = (i_data3 >> 8) & 0xff;
+                        tx_data3_L = i_data3 & 0xff;
+                    tx_data[0] = id;                                    // ID
+                    tx_data[1] = 1;                                     // mode (1: response)
+                    tx_data[2] = tx_data1_U;                            // response value1 upper 8bit
+                    tx_data[3] = tx_data1_L;                            // response value1 lower 8bit
+                    tx_data[4] = tx_data2_U;                            // response value2 upper 8bit
+                    tx_data[5] = tx_data2_L;                            // response value2 lower 8bit
+                    tx_data[6] = tx_data3_U;                            // response value3 upper 8bit
+                    tx_data[7] = tx_data3_L;                            // response value3 lower 8bit
+                    can.write(CANMessage(1330, tx_data, 8));
+                }
+            }
+            myled = 1; // LED is ON
+            wait (0.01);
+        } else {
+            myled = 0; // LED is OFF
+        }
+    } // while 
+}
\ No newline at end of file