T Nara / Mbed 2 deprecated ros_lib_kinetic

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 BufferedSerial

main.cpp@3:67c0e888fb36, 2018-12-01 (annotated)

Committer:
tknara
Date:
Sat Dec 01 11:49:43 2018 +0000
Revision:
3:67c0e888fb36
Parent:
2:0db4556d847c
Child:
4:bcdd99711326
?????????????????????????????????????; ????????????????

Who changed what in which revision?

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