Diff: main.cpp

Revision:

1:a9f2d00d7a56

Parent:

0:db35d0ca1d41

--- a/main.cpp	Wed Mar 17 17:55:31 2021 +0000
+++ b/main.cpp	Thu Jun 10 06:14:49 2021 +0000
@@ -3,6 +3,7 @@
 // Created by Atsushi Kakogawa, 2019.09.19
 // Edited by Yoshimichi Oka, 2020.2.5
 // Modified by Atsushi Kakogawa, 2020.03.22
+// Modified by Yoshimichi Oka, 2021.06.10
 // Department of Robotics, Ritsumeikan University, Japan
 #include "mbed.h"
 #include "CAN.h"
@@ -47,6 +48,9 @@
                 } else if (msg.data[1] == 2) {
                     target_ang1 = (msg.data[3] << 8) + msg.data[4];
                     mode = 2; 
+                } else if (msg.data[1] == 3) {
+                    target_ang1 = (msg.data[3] << 8) + msg.data[4];
+                    mode = 3; 
                 } else if (msg.data[1] == 1) {                          // mode indentify (1: response)
                     int i_data1 = AD_crt.read()*1000; 
                         tx_data1_U = (i_data1 >> 8) & 0xff;
@@ -104,7 +108,7 @@
         pe = e;
         //pang = ang;
         
-    } else if (mode==2){
+    } else if (mode==2){   //脱力制御
         /*
         for (i=1; i<200; i++) {
             can_reader();
@@ -147,7 +151,61 @@
             }
             */
             //can_reader();
-    }
+    } else if (mode==3){   //角度制御+脱力制御（T字管用）
+      //if(id == 13){
+            if (e < 0) {
+                mdir1 = 1;
+                mdir2 = 0;
+            } else if (e > 0) {
+                mdir1 = 1;
+                mdir2 = 1;
+            } else {
+                mdir1 = 0;
+                mdir2 = 0;
+            } 
+            
+            ang = (potensio1.read())*330;
+            e = target_ang1 - ang;
+            de = (e - pe)/dt;
+            
+            DA_crt = kp*abs(e) + kd*abs(de);
+            if (DA_crt > 1){
+                DA_crt = 1;
+            } else if (DA_crt <= 0){
+                DA_crt = 0;
+                }
+            pe = e;        
+            /*
+        }else {
+            //mdir1 = 0;
+            if (e < 0) {
+                mdir1 = 1;
+                mdir2 = 0;
+            } else if (e > 0) {
+                mdir1 = 1;
+                mdir2 = 1;
+            } else {
+                mdir1 = 0;
+                mdir2 = 0;
+            } 
+            
+            ang = (potensio1.read())*330;
+            e = target_ang1 - ang;
+            de = (e - pe)/dt;
+            //de = ang - pang;
+            DA_crt = kp*abs(e) + kd*abs(de);
+            if (DA_crt > 1){
+                DA_crt = 1;
+            } else if (DA_crt <= 0){
+                DA_crt = 0;
+                }
+                
+            pe = e;
+
+            
+            }   
+        */
+        }      
 }
 
 int main() {