Diff: main.cpp

Revision:

0:dface1689d87

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jul 17 05:58:55 2021 +0000
@@ -0,0 +1,168 @@
+#include "mbed.h"
+#include "EC.h"
+#include "CalPID.h"
+#include "MotorController.h"
+
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <geometry_msgs/Point.h>
+
+#define RESOLUTION 500
+#define pi 3.1415926
+#define DELTA_T 0.001
+#define DUTY_MAX 0.8
+#define OMEGA_MAX 3
+
+//Serial pc(USBTX, USBRX);
+CAN can(p30, p29);
+Ticker ticker;
+Ticker motor_ticker;
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+char can_now[1];
+char can_new[1];
+int status_now = 0;
+int status_new = 0;
+double theta=0;
+double omega=0;
+CANMessage msg;
+
+DigitalIn button1(p15);
+DigitalIn button2(p16);
+
+//角度制御
+Ec2multi ec = Ec2multi(p28, p27, RESOLUTION);
+CalPID speed_pid(0.9281,0,0.0003086,DELTA_T,DUTY_MAX);
+CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX);
+CalPID angle_omega_pid(6.726,0,0.0007063,DELTA_T,OMEGA_MAX);
+MotorController motor= MotorController(p25,p26, 0.001, ec, speed_pid, angle_duty_pid, angle_omega_pid);
+
+//
+
+void status_read(int id)
+{
+    if (can.read(msg) && msg.id == id)
+    {
+        status_now = (int)msg.data[0];
+    }
+    if(status_new<status_now)status_new=status_now;
+}
+
+void status_write(int id)
+{
+    can_new[0] = status_new & 255;
+    can.write(CANMessage(id, can_new, 1));
+}
+
+void status()
+{
+    status_read(1);
+    status_write(0);
+}
+
+void led_light()
+{
+    led2 = status_now % 2;
+    led3 = status_now / 2 % 2;
+    led4 = status_now / 4 % 2;
+}
+
+void motor_move(){
+    //motor.AcOmega(theta);
+    motor.Sc(omega);
+}
+
+float Arrow_NO=21; //arrow number + fire
+float L=21; //length
+float X=21; //rad
+
+void messageCallback(const geometry_msgs::Point &self_position)
+{
+    Arrow_NO = self_position.x;
+    L = self_position.y;
+    X = self_position.z;
+    led4=1;
+}
+
+ros::NodeHandle nh;
+ros::Subscriber<geometry_msgs::Point> sub("/coordinate",&messageCallback);
+
+int main()
+{
+    nh.getHardware()->setBaud(115200);
+    nh.initNode();
+    nh.subscribe(sub);
+
+    
+    motor.setDutyLimit(0.3);
+    button1.mode(PullUp);
+    button2.mode(PullUp);
+
+    led1 = 1;
+    wait(0.5);
+    led1 = 0;
+    can.frequency(1000000);
+    ticker.attach(&status, 0.001);
+    motor_ticker.attach(&motor_move, 0.001);
+    led4=0;
+    while (1)
+    {
+        nh.spinOnce();
+        if(Arrow_NO==1001)led1=1;
+        else led1=0;
+        if(L==6.545)led2=1;
+        else led2=0;
+        float pi_ = 3.1415;
+        if(X==pi_/5)led3=1;
+        else led3=0;
+        //pc.printf("%f %f %f\n",Arrow_NO,L,X);
+        //pc.printf("now state %d new %d, %f\r\n",status_now,status_new,theta);
+        switch (status_now)
+        {
+        case 0:
+            break;
+        case 1:
+            break;
+        case 2:
+            theta=pi;//掴む位置
+            omega=1;
+            if (button1==0)
+            {
+                wait(1);
+                status_new = 3;
+            }
+            break;
+        case 3:
+            break;
+        case 4:
+            theta=pi/4;//半開で矢を落とす位置
+            if (button1==0)
+            {
+                wait(1);
+                status_new = 5;
+            }
+            break;
+        case 5:
+            break;
+        case 6:
+            theta=0;//打つ位置
+            if (button1==0)
+            {
+                wait(1);
+                status_new = 7;
+            }
+            break;
+        case 7:
+            break;
+        case 8:
+            //打つ
+            break;
+        }
+        //led_light();
+        //wait_ms(1);
+         wait(0.1);
+    }
+}
\ No newline at end of file