T Nara / Mbed 2 deprecated ros_lib_kinetic

hairo

Dependencies:   mbed BufferedSerial

main.cpp@4:bcdd99711326, 2018-12-04 (annotated)

Committer:
tknara
Date:
Tue Dec 04 00:14:32 2018 +0000
Revision:
4:bcdd99711326
Parent:
3:67c0e888fb36 
test

Who changed what in which revision?

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