7th_DENSOU
/
TR_arrow1
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: mbed CalPID MotorController Encoder_GEAR ros_lib_melodic
Revision 0:dface1689d87, committed 2021-07-17
- Comitter:
- maxnagazumi
- Date:
- Sat Jul 17 05:58:55 2021 +0000
- Commit message:
- mbed
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CalPID.lib Sat Jul 17 05:58:55 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/7th_DENSOU/code/CalPID/#0668932f9d5d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder_GERA.lib Sat Jul 17 05:58:55 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/7th_DENSOU/code/Encoder_GEAR/#7ee4ff55191d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorController.lib Sat Jul 17 05:58:55 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/7th_DENSOU/code/MotorController/#d7677caf013a
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Jul 17 05:58:55 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_melodic.lib Sat Jul 17 05:58:55 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_melodic/#da82487f547e