NagaokaRoboticsClub_mbedTeam
廃炉ロボコン2018用プログラム ROS→stm32（ROSからのシリアルをCANに変換する）→MD
Dependencies: ros_lib_kinetic
Revision 0:e5287b509fe9, committed 2018-12-04
- Comitter:
- tknara
- Date:
- Tue Dec 04 06:36:29 2018 +0000
- Child:
- 1:e15fbee169e7
- Commit message:
- worked;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Dec 04 06:36:29 2018 +0000
@@ -0,0 +1,149 @@
+#include "mbed.h"
+//#include "ikarashiMDC.h"
+#include "algorithm"
+#include <ros.h>
+#include <sensor_msgs/Joy.h>
+#include <std_msgs/String.h>
+#define TIMEOUT 0.4
+DigitalOut led(LED1);
+CAN can(PA_11,PA_12);
+DigitalOut led2(PA_0);
+Timeout timeout;
+
+double wheelSpeed[4]={0};
+double joyaxes[8];
+double joybuttons[11];
+double waterPower[4]={0};
+double armPower[4]={0};
+CANMessage msg;
+std_msgs::String debug_msg;
+void reset(){
+ //NVIC_SystemReset();
+}
+int Button(int a,int b){
+ if((a==1)&&(b==0)) return 1;
+ else if((a==0)&&(b==1)) return -1;
+ else return 0;
+}
+void joy_callback(const sensor_msgs::Joy &joy)
+{
+ int i;
+ led = !led;
+ for(i=0;i<8;i++)
+ {
+ joyaxes[i]=joy.axes[i];
+ /*
+ 0 Left/Right Axis stick left
+ 1 Up/Down Axis stick left
+ 2 LT
+ 3 Left/Right Axis stick right
+ 4 Up/Down Axis stick right
+ 5 RT
+ 6 cross key left/right
+ 7 cross key up/down
+ */
+ }
+ for(i=0;i<11;i++)
+ {
+ joybuttons[i]=joy.buttons[i];
+ /*
+ 0 A
+ 1 B
+ 2 X
+ 3 Y
+ 4 LB
+ 5 RB
+ 6 back
+ 7 start
+ 8 power
+ 9 Button stick left
+ 10 Button stick right
+ */
+ }
+}
+double cropped_speed(double speed)
+{
+ return std::min(1.0,std::max(-1.0,speed));
+}
+void setWheelSpeed()
+{
+
+ if(joybuttons[9]==1)
+ {
+ wheelSpeed[0]=joyaxes[0];
+ wheelSpeed[2]=joyaxes[1];
+ }else {
+ wheelSpeed[0]=joyaxes[1]*-1;
+ wheelSpeed[2]=joyaxes[1]*-1;
+ }
+ if(joybuttons[10]==1)
+ {
+ wheelSpeed[1]=joyaxes[3]*-1;
+ wheelSpeed[3]=joyaxes[4]*-1;
+ }else{
+ wheelSpeed[1]=joyaxes[4];
+ wheelSpeed[3]=joyaxes[4];
+ }
+ msg.id = 1;
+ msg.len = 4;
+ for(int i=0;i<4;i++)
+ {
+ msg.data[i] = ((cropped_speed(wheelSpeed[i])+1.0)/2.0)*253;
+ }
+ can.write(msg);
+}
+void setWaterSpeed()
+{
+ waterPower[0] = Button(joybuttons[0],joybuttons[1]);
+ waterPower[1] = Button(joybuttons[2],joybuttons[3]);
+ waterPower[2] = Button(joybuttons[4],joybuttons[5]);
+ msg.id = 2;
+ msg.len = 4;
+ for(int i=0;i<4;i++)
+ {
+ msg.data[i] = ((cropped_speed(waterPower[i])+1.0)/2.0)*253;
+ }
+ can.write(msg);
+}
+void setArmSpeed()
+{
+ armPower[0]=joyaxes[6];
+ armPower[1]=joyaxes[7];
+ msg.id = 3;
+ msg.len = 4;
+ for(int i=0;i<4;i++)
+ {
+ msg.data[i] = ((cropped_speed(armPower[i])+1.0)/2.0)*253;
+ }
+ can.write(msg);
+}
+int main(){
+ bool flag=false;
+ ros::NodeHandle nh;
+ ros::Subscriber<sensor_msgs::Joy> sub("joy", &joy_callback);
+ ros::Publisher pub("chatter", &debug_msg);
+ nh.getHardware()->setBaud(921600);
+ nh.initNode();
+ nh.subscribe(sub);
+ nh.advertise(pub);
+ can.frequency(125000);
+ while(1)
+ {
+ nh.spinOnce();
+ setWheelSpeed();
+ setWaterSpeed();
+ setArmSpeed();
+ if(can.tderror())
+ {
+ led2 = !led2;
+ if(!flag){
+ flag=true;
+ timeout.attach(&reset,TIMEOUT);
+ }
+ }else{
+ flag=false;
+ timeout.detach();
+ }
+ wait(0.01);
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-dev.lib Tue Dec 04 06:36:29 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/tknara/code/mbed-dev2/#95ae1c16d509
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Tue Dec 04 06:36:29 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f