NagaokaRoboticsClub_mbedTeam / hairo

廃炉ロボコン2018用プログラム ROS→stm32（ROSからのシリアルをCANに変換する）→MD

Dependencies:   ros_lib_kinetic

main.cpp@1:e15fbee169e7, 2018-12-04 (annotated)

Committer:
tknara
Date:
Tue Dec 04 09:00:44 2018 +0000
Revision:
1:e15fbee169e7
Parent:
0:e5287b509fe9 
added mode change;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tknara 0:e5287b509fe9 1 #include "mbed.h"
tknara 0:e5287b509fe9 2 #include "algorithm"
tknara 0:e5287b509fe9 3 #include <ros.h>
tknara 0:e5287b509fe9 4 #include <sensor_msgs/Joy.h>
tknara 0:e5287b509fe9 5 #include <std_msgs/String.h>
tknara 0:e5287b509fe9 6 #define TIMEOUT 0.4
tknara 0:e5287b509fe9 7 DigitalOut led(LED1);
tknara 0:e5287b509fe9 8 CAN can(PA_11,PA_12);
tknara 0:e5287b509fe9 9 DigitalOut led2(PA_0);
tknara 0:e5287b509fe9 10 Timeout timeout;
tknara 1:e15fbee169e7 11 int modeflag=0;
tknara 0:e5287b509fe9 12 double wheelSpeed[4]={0};
tknara 0:e5287b509fe9 13 double joyaxes[8];
tknara 0:e5287b509fe9 14 double joybuttons[11];
tknara 0:e5287b509fe9 15 double waterPower[4]={0};
tknara 0:e5287b509fe9 16 double armPower[4]={0};
tknara 0:e5287b509fe9 17 CANMessage msg;
tknara 0:e5287b509fe9 18 std_msgs::String debug_msg;
tknara 0:e5287b509fe9 19 void reset(){
tknara 0:e5287b509fe9 20 //NVIC_SystemReset();
tknara 0:e5287b509fe9 21 }
tknara 0:e5287b509fe9 22 int Button(int a,int b){
tknara 0:e5287b509fe9 23 if((a==1)&&(b==0)) return 1;
tknara 0:e5287b509fe9 24 else if((a==0)&&(b==1)) return -1;
tknara 0:e5287b509fe9 25 else return 0;
tknara 0:e5287b509fe9 26 }
tknara 0:e5287b509fe9 27 void joy_callback(const sensor_msgs::Joy &joy)
tknara 0:e5287b509fe9 28 {
tknara 0:e5287b509fe9 29 int i;
tknara 0:e5287b509fe9 30 led = !led;
tknara 0:e5287b509fe9 31 for(i=0;i<8;i++)
tknara 0:e5287b509fe9 32 {
tknara 0:e5287b509fe9 33 joyaxes[i]=joy.axes[i];
tknara 0:e5287b509fe9 34 /*
tknara 0:e5287b509fe9 35 0 Left/Right Axis stick left
tknara 0:e5287b509fe9 36 1 Up/Down Axis stick left
tknara 0:e5287b509fe9 37 2 LT
tknara 0:e5287b509fe9 38 3 Left/Right Axis stick right
tknara 0:e5287b509fe9 39 4 Up/Down Axis stick right
tknara 0:e5287b509fe9 40 5 RT
tknara 0:e5287b509fe9 41 6 cross key left/right
tknara 0:e5287b509fe9 42 7 cross key up/down
tknara 0:e5287b509fe9 43 */
tknara 0:e5287b509fe9 44 }
tknara 0:e5287b509fe9 45 for(i=0;i<11;i++)
tknara 0:e5287b509fe9 46 {
tknara 0:e5287b509fe9 47 joybuttons[i]=joy.buttons[i];
tknara 0:e5287b509fe9 48 /*
tknara 0:e5287b509fe9 49 0 A
tknara 0:e5287b509fe9 50 1 B
tknara 0:e5287b509fe9 51 2 X
tknara 0:e5287b509fe9 52 3 Y
tknara 0:e5287b509fe9 53 4 LB
tknara 0:e5287b509fe9 54 5 RB
tknara 0:e5287b509fe9 55 6 back
tknara 0:e5287b509fe9 56 7 start
tknara 0:e5287b509fe9 57 8 power
tknara 0:e5287b509fe9 58 9 Button stick left
tknara 0:e5287b509fe9 59 10 Button stick right
tknara 0:e5287b509fe9 60 */
tknara 0:e5287b509fe9 61 }
tknara 0:e5287b509fe9 62 }
tknara 0:e5287b509fe9 63 double cropped_speed(double speed)
tknara 0:e5287b509fe9 64 {
tknara 0:e5287b509fe9 65 return std::min(1.0,std::max(-1.0,speed));
tknara 0:e5287b509fe9 66 }
tknara 0:e5287b509fe9 67 void setWheelSpeed()
tknara 0:e5287b509fe9 68 {
tknara 1:e15fbee169e7 69 static int mode=0,toggle=0;
tknara 1:e15fbee169e7 70 if((joybuttons[7]==1)&&(toggle==0)&&(mode==0))
tknara 0:e5287b509fe9 71 {
tknara 1:e15fbee169e7 72 toggle=1;
tknara 1:e15fbee169e7 73 mode=1;
tknara 1:e15fbee169e7 74 }else if((joybuttons[7]==1)&&(toggle==0)&&(mode==1)){
tknara 1:e15fbee169e7 75 toggle=1;
tknara 1:e15fbee169e7 76 mode=0;
tknara 1:e15fbee169e7 77 }else{
tknara 1:e15fbee169e7 78 toggle=0;
tknara 1:e15fbee169e7 79 }
tknara 1:e15fbee169e7 80 /*normal mode*/
tknara 1:e15fbee169e7 81 if(mode==0)
tknara 1:e15fbee169e7 82 {
tknara 0:e5287b509fe9 83 wheelSpeed[0]=joyaxes[1]*-1;
tknara 1:e15fbee169e7 84 wheelSpeed[1]=joyaxes[4];
tknara 0:e5287b509fe9 85 wheelSpeed[2]=joyaxes[1]*-1;
tknara 0:e5287b509fe9 86 wheelSpeed[3]=joyaxes[4];
tknara 1:e15fbee169e7 87 }else{
tknara 1:e15fbee169e7 88 wheelSpeed[0]=joyaxes[1]*-1;
tknara 1:e15fbee169e7 89 wheelSpeed[1]=joyaxes[1];
tknara 1:e15fbee169e7 90 wheelSpeed[2]=joyaxes[3]*0.3;
tknara 1:e15fbee169e7 91 wheelSpeed[3]=joyaxes[3]*0.3;
tknara 0:e5287b509fe9 92 }
tknara 0:e5287b509fe9 93 msg.id = 1;
tknara 0:e5287b509fe9 94 msg.len = 4;
tknara 0:e5287b509fe9 95 for(int i=0;i<4;i++)
tknara 0:e5287b509fe9 96 {
tknara 0:e5287b509fe9 97 msg.data[i] = ((cropped_speed(wheelSpeed[i])+1.0)/2.0)*253;
tknara 0:e5287b509fe9 98 }
tknara 0:e5287b509fe9 99 can.write(msg);
tknara 0:e5287b509fe9 100 }
tknara 0:e5287b509fe9 101 void setWaterSpeed()
tknara 0:e5287b509fe9 102 {
tknara 0:e5287b509fe9 103 waterPower[0] = Button(joybuttons[0],joybuttons[1]);
tknara 0:e5287b509fe9 104 waterPower[1] = Button(joybuttons[2],joybuttons[3]);
tknara 0:e5287b509fe9 105 waterPower[2] = Button(joybuttons[4],joybuttons[5]);
tknara 0:e5287b509fe9 106 msg.id = 2;
tknara 0:e5287b509fe9 107 msg.len = 4;
tknara 0:e5287b509fe9 108 for(int i=0;i<4;i++)
tknara 0:e5287b509fe9 109 {
tknara 0:e5287b509fe9 110 msg.data[i] = ((cropped_speed(waterPower[i])+1.0)/2.0)*253;
tknara 0:e5287b509fe9 111 }
tknara 0:e5287b509fe9 112 can.write(msg);
tknara 0:e5287b509fe9 113 }
tknara 0:e5287b509fe9 114 void setArmSpeed()
tknara 0:e5287b509fe9 115 {
tknara 0:e5287b509fe9 116 armPower[0]=joyaxes[6];
tknara 0:e5287b509fe9 117 armPower[1]=joyaxes[7];
tknara 0:e5287b509fe9 118 msg.id = 3;
tknara 0:e5287b509fe9 119 msg.len = 4;
tknara 0:e5287b509fe9 120 for(int i=0;i<4;i++)
tknara 0:e5287b509fe9 121 {
tknara 0:e5287b509fe9 122 msg.data[i] = ((cropped_speed(armPower[i])+1.0)/2.0)*253;
tknara 0:e5287b509fe9 123 }
tknara 0:e5287b509fe9 124 can.write(msg);
tknara 0:e5287b509fe9 125 }
tknara 0:e5287b509fe9 126 int main(){
tknara 0:e5287b509fe9 127 bool flag=false;
tknara 0:e5287b509fe9 128 ros::NodeHandle nh;
tknara 0:e5287b509fe9 129 ros::Subscriber<sensor_msgs::Joy> sub("joy", &joy_callback);
tknara 0:e5287b509fe9 130 ros::Publisher pub("chatter", &debug_msg);
tknara 0:e5287b509fe9 131 nh.getHardware()->setBaud(921600);
tknara 0:e5287b509fe9 132 nh.initNode();
tknara 0:e5287b509fe9 133 nh.subscribe(sub);
tknara 0:e5287b509fe9 134 nh.advertise(pub);
tknara 0:e5287b509fe9 135 can.frequency(125000);
tknara 0:e5287b509fe9 136 while(1)
tknara 0:e5287b509fe9 137 {
tknara 0:e5287b509fe9 138 nh.spinOnce();
tknara 0:e5287b509fe9 139 setWheelSpeed();
tknara 0:e5287b509fe9 140 setWaterSpeed();
tknara 0:e5287b509fe9 141 setArmSpeed();
tknara 0:e5287b509fe9 142 if(can.tderror())
tknara 0:e5287b509fe9 143 {
tknara 0:e5287b509fe9 144 led2 = !led2;
tknara 0:e5287b509fe9 145 if(!flag){
tknara 0:e5287b509fe9 146 flag=true;
tknara 0:e5287b509fe9 147 timeout.attach(&reset,TIMEOUT);
tknara 0:e5287b509fe9 148 }
tknara 0:e5287b509fe9 149 }else{
tknara 0:e5287b509fe9 150 flag=false;
tknara 0:e5287b509fe9 151 timeout.detach();
tknara 0:e5287b509fe9 152 }
tknara 0:e5287b509fe9 153 wait(0.01);
tknara 0:e5287b509fe9 154 }
tknara 0:e5287b509fe9 155 }