7th_DENSOU / Mbed 2 deprecated nhk_2021motorAB_

motorAB

Dependencies:   mbed ros_lib_melodic

main.cpp@1:163629043391, 2021-05-15 (annotated)

Committer:
koheim
Date:
Sat May 15 08:34:17 2021 +0000
Revision:
1:163629043391
Parent:
0:cca61e773cbb 
nhk_motor(AB)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:cca61e773cbb 1 #include "mbed.h"
la00noix 0:cca61e773cbb 2 #include "EC.h"
la00noix 0:cca61e773cbb 3 #include <ros.h>
la00noix 0:cca61e773cbb 4 //#include <geometry_msgs/Inertia.h>
la00noix 0:cca61e773cbb 5 #include <geometry_msgs/Point.h>
la00noix 0:cca61e773cbb 6 #include <geometry_msgs/Quaternion.h>
la00noix 0:cca61e773cbb 7 #include <std_msgs/Int8.h>
la00noix 0:cca61e773cbb 8
la00noix 0:cca61e773cbb 9 SpeedControl motorA(p7,p6,NC,360,0.05,p21,p22);
la00noix 0:cca61e773cbb 10 SpeedControl motorB(p10,p11,NC,360,0.05,p23,p24);
la00noix 0:cca61e773cbb 11
la00noix 0:cca61e773cbb 12
la00noix 0:cca61e773cbb 13 DigitalOut led(LED1);
la00noix 0:cca61e773cbb 14
la00noix 0:cca61e773cbb 15 Ticker ticker;
la00noix 0:cca61e773cbb 16 Ticker ticker2;
la00noix 0:cca61e773cbb 17 Timer timer;
la00noix 0:cca61e773cbb 18
la00noix 0:cca61e773cbb 19 double vA=0,vB=0;
la00noix 0:cca61e773cbb 20 bool waiting=0;
la00noix 0:cca61e773cbb 21 double time1;
la00noix 0:cca61e773cbb 22
la00noix 0:cca61e773cbb 23 int k=0;
la00noix 0:cca61e773cbb 24 int check_k=0;
la00noix 0:cca61e773cbb 25
la00noix 0:cca61e773cbb 26 void calOmega(){
la00noix 0:cca61e773cbb 27 motorA.CalOmega();
la00noix 0:cca61e773cbb 28 motorB.CalOmega();
la00noix 0:cca61e773cbb 29 }
la00noix 0:cca61e773cbb 30
la00noix 0:cca61e773cbb 31 ros::NodeHandle nh;
la00noix 0:cca61e773cbb 32
la00noix 0:cca61e773cbb 33 //bool tickerstop=1;
la00noix 0:cca61e773cbb 34
la00noix 0:cca61e773cbb 35 //void messageCallback(const geometry_msgs::Inertia& msg){
la00noix 0:cca61e773cbb 36 void messageCallback(const geometry_msgs::Point& msg){
la00noix 0:cca61e773cbb 37
la00noix 0:cca61e773cbb 38 vA=msg.x;
la00noix 0:cca61e773cbb 39 vB=msg.y;
la00noix 0:cca61e773cbb 40 bool reset=msg.z;
la00noix 0:cca61e773cbb 41
la00noix 0:cca61e773cbb 42 if(reset){
la00noix 0:cca61e773cbb 43 vA=0;
la00noix 0:cca61e773cbb 44 vB=0;
la00noix 0:cca61e773cbb 45 motorA.reset();
la00noix 0:cca61e773cbb 46 motorB.reset();
la00noix 0:cca61e773cbb 47 }
la00noix 0:cca61e773cbb 48 if(waiting==1){
la00noix 0:cca61e773cbb 49 timer.start();
la00noix 0:cca61e773cbb 50 waiting=0;
la00noix 0:cca61e773cbb 51 }else{
la00noix 0:cca61e773cbb 52 time1=timer.read();
la00noix 0:cca61e773cbb 53 }
la00noix 0:cca61e773cbb 54 }
la00noix 0:cca61e773cbb 55
la00noix 0:cca61e773cbb 56 //ros::Subscriber<geometry_msgs::Inertia> motion_sub("motion",&messageCallback);
la00noix 0:cca61e773cbb 57 ros::Subscriber<geometry_msgs::Point> motion_sub("motion_motorAB",&messageCallback);
la00noix 0:cca61e773cbb 58
la00noix 0:cca61e773cbb 59 geometry_msgs::Quaternion data;
la00noix 0:cca61e773cbb 60 ros::Publisher debug_pub("debug_duty_ab", &data);
la00noix 0:cca61e773cbb 61
la00noix 0:cca61e773cbb 62 std_msgs::Int8 msg_main;
koheim 1:163629043391 63 ros::Publisher pub("main_checkAB",&msg_main);
la00noix 0:cca61e773cbb 64
la00noix 0:cca61e773cbb 65
la00noix 0:cca61e773cbb 66 void debug(){
la00noix 0:cca61e773cbb 67 data.x=motorA.getOmega();
la00noix 0:cca61e773cbb 68 data.y=motorB.getOmega();
la00noix 0:cca61e773cbb 69 data.z=motorA.duty;
la00noix 0:cca61e773cbb 70 data.w=motorB.duty;
la00noix 0:cca61e773cbb 71 debug_pub.publish( &data );
la00noix 0:cca61e773cbb 72 }
la00noix 0:cca61e773cbb 73
la00noix 0:cca61e773cbb 74 int k_1=0;
la00noix 0:cca61e773cbb 75 void spin(){
la00noix 0:cca61e773cbb 76 /*k_1++;
la00noix 0:cca61e773cbb 77 if(k_1>50){
la00noix 0:cca61e773cbb 78 k_1=0;
la00noix 0:cca61e773cbb 79 debug();
la00noix 0:cca61e773cbb 80 }*/
la00noix 0:cca61e773cbb 81 debug();
la00noix 0:cca61e773cbb 82 check_k++;
la00noix 0:cca61e773cbb 83 if(check_k>100){
la00noix 0:cca61e773cbb 84 check_k=0;
la00noix 0:cca61e773cbb 85 }
la00noix 0:cca61e773cbb 86 msg_main.data=check_k;
la00noix 0:cca61e773cbb 87 pub.publish(&msg_main);
la00noix 0:cca61e773cbb 88 nh.spinOnce();
la00noix 0:cca61e773cbb 89 }
la00noix 0:cca61e773cbb 90
la00noix 0:cca61e773cbb 91 int main() {
la00noix 0:cca61e773cbb 92 //nh.getHardware()->setBaud(921600);
la00noix 0:cca61e773cbb 93 nh.getHardware()->setBaud(115200);
la00noix 0:cca61e773cbb 94 nh.initNode();
la00noix 0:cca61e773cbb 95 nh.subscribe(motion_sub);
la00noix 0:cca61e773cbb 96 nh.advertise(pub);
la00noix 0:cca61e773cbb 97 nh.advertise(debug_pub);
la00noix 0:cca61e773cbb 98
la00noix 0:cca61e773cbb 99 timer.start();
la00noix 0:cca61e773cbb 100
la00noix 0:cca61e773cbb 101 motorA.pwm_F_.period_us(100);
la00noix 0:cca61e773cbb 102 motorA.pwm_B_.period_us(100);
la00noix 0:cca61e773cbb 103 motorB.pwm_F_.period_us(100);
la00noix 0:cca61e773cbb 104 motorB.pwm_B_.period_us(100);
la00noix 0:cca61e773cbb 105
la00noix 0:cca61e773cbb 106
la00noix 0:cca61e773cbb 107
la00noix 0:cca61e773cbb 108
la00noix 0:cca61e773cbb 109
la00noix 0:cca61e773cbb 110 //速度に対するpid係数
la00noix 0:cca61e773cbb 111 /*motorA.setPDparam(0.1,0.001);
la00noix 0:cca61e773cbb 112 motorB.setPDparam(0.1,0.001);*/
la00noix 0:cca61e773cbb 113 motorA.setPDparam(0.09,0);
la00noix 0:cca61e773cbb 114 motorB.setPDparam(0.05,0);
la00noix 0:cca61e773cbb 115
la00noix 0:cca61e773cbb 116
la00noix 0:cca61e773cbb 117 //Cは傾き，Dは切片(縦デューティー，横入力値 のグラフにおいて) C=1/c, D=d
la00noix 0:cca61e773cbb 118 motorA.setDOconstant(295,0,295,0);
la00noix 0:cca61e773cbb 119 motorB.setDOconstant(305,0,305,0);
la00noix 0:cca61e773cbb 120
la00noix 0:cca61e773cbb 121
la00noix 0:cca61e773cbb 122 //合わせたver
la00noix 0:cca61e773cbb 123 /*motorA.setDOconstant(250,0,256,0);
la00noix 0:cca61e773cbb 124 motorB.setDOconstant(252,0,267,0);*/
la00noix 0:cca61e773cbb 125
la00noix 0:cca61e773cbb 126
la00noix 0:cca61e773cbb 127
la00noix 0:cca61e773cbb 128
la00noix 0:cca61e773cbb 129
la00noix 0:cca61e773cbb 130 ticker.attach(&calOmega,0.05);
la00noix 0:cca61e773cbb 131 wait_ms(5);
la00noix 0:cca61e773cbb 132 ticker2.attach(&spin,0.01);
la00noix 0:cca61e773cbb 133
la00noix 0:cca61e773cbb 134 while(1) {
la00noix 0:cca61e773cbb 135 if(vA)motorA.Sc(vA);
la00noix 0:cca61e773cbb 136 else motorA.stop();
la00noix 0:cca61e773cbb 137 if(vB)motorB.Sc(vB);
la00noix 0:cca61e773cbb 138 else motorB.stop();
la00noix 0:cca61e773cbb 139 k++;
la00noix 0:cca61e773cbb 140 if((k>5000)&&(waiting==0)){
la00noix 0:cca61e773cbb 141 double time_a=timer.read()-time1;
la00noix 0:cca61e773cbb 142 if(time_a>0.5){
la00noix 0:cca61e773cbb 143 led=0;
la00noix 0:cca61e773cbb 144 vA=0;
la00noix 0:cca61e773cbb 145 vB=0;
la00noix 0:cca61e773cbb 146 motorA.reset();
la00noix 0:cca61e773cbb 147 motorB.reset();
la00noix 0:cca61e773cbb 148
la00noix 0:cca61e773cbb 149 waiting=1;
la00noix 0:cca61e773cbb 150 timer.stop();
la00noix 0:cca61e773cbb 151 timer.reset();
la00noix 0:cca61e773cbb 152 time1=0;
la00noix 0:cca61e773cbb 153 }else{
la00noix 0:cca61e773cbb 154 led=1;
la00noix 0:cca61e773cbb 155 }
la00noix 0:cca61e773cbb 156 }
la00noix 0:cca61e773cbb 157 }
la00noix 0:cca61e773cbb 158 }