hairo
Dependencies: mbed BufferedSerial
Revision 2:0db4556d847c, committed 2018-11-30
- Comitter:
- tknara
- Date:
- Fri Nov 30 07:26:58 2018 +0000
- Parent:
- 1:a849bf78d77f
- Child:
- 3:67c0e888fb36
- Commit message:
- callbackwokarukusuru;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ikarashiMDC.lib Fri Nov 30 07:26:58 2018 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/NHK-Robocon2016_Nagaoka_B_Team/code/ikarashiMDC/#ede155e45acc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Nov 30 07:26:58 2018 +0000 @@ -0,0 +1,116 @@ +#include <ros.h>; +#include <sensor_msgs/Joy.h>; +#include <std_msgs/String.h>; +#include "mbed.h" +#include "ikarashiMDC.h" +DigitalOut led(LED1); +CAN can(PA_11,PA_12); +DigitalOut led2(PA_0); +Ticker ticker; +ikarashiMDC wheel[]{ + ikarashiMDC(1,0,SM,&can), + ikarashiMDC(1,1,SM,&can), + ikarashiMDC(1,2,SM,&can), + ikarashiMDC(1,3,SM,&can) +}; +ikarashiMDC arm[]{ + ikarashiMDC(2,0,SM,&can), + ikarashiMDC(2,1,SM,&can), + ikarashiMDC(2,2,SM,&can), + ikarashiMDC(2,3,SM,&can) +}; +ikarashiMDC water[]{ + ikarashiMDC(3,0,SM,&can), + ikarashiMDC(3,1,SM,&can), + ikarashiMDC(3,2,SM,&can), + ikarashiMDC(3,3,SM,&can) +}; +double wheelSpeed[4]={0}; +double joyaxes[4]; +double waterPower[4]={0}; +double armPower[4]={0}; +CANMessage msg; +std_msgs::String debug_msg; +void joy_callback(const sensor_msgs::Joy &joy) +{ + led = ! led; + wheelSpeed[0]=joy.axes[1]*-1; + wheelSpeed[1]=joy.axes[4]; + wheelSpeed[2]=joy.axes[1]*-1; + wheelSpeed[3]=joy.axes[4]; + armPower[0]=joy.axes[6]; + armPower[1]=joy.axes[7]; + if((joy.buttons[0]==1)&&(joy.buttons[3]==0)){ + waterPower[0]=1; + }else if((joy.buttons[0]==0)&&(joy.buttons[3]==1)){ + waterPower[0]=-1; + }else if((joy.buttons[0]==0)&&(joy.buttons[3]==0)){ + waterPower[0]=0; + }else { + waterPower[0]=0; + } + + if((joy.buttons[1]==1)&&(joy.buttons[2]==0)){ + waterPower[1]=1; + }else if((joy.buttons[1]==0)&&(joy.buttons[2]==1)){ + waterPower[1]=-1; + }else if((joy.buttons[1]==0)&&(joy.buttons[2]==0)){ + waterPower[1]=0; + }else { + waterPower[1]=0; + } + + if((joy.buttons[4]==1)&&(joy.buttons[5]==0)){ + waterPower[2]=1; + }else if((joy.buttons[4]==0)&&(joy.buttons[5]==1)){ + waterPower[2]=-1; + }else if ((joy.buttons[4]==0)&&(joy.buttons[5]==1)){ + waterPower[2]=0; + }else { + waterPower[2]=0; + } +} + +int main(){ + int i; + int counter=0; + 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(500000); + for(i=0;i<4;i++) + { + wheel[i].braking = true; + water[i].braking = true; + arm[i].braking = true; + } + wait(0.5); + while(1) + { + nh.spinOnce(); + for(i=0;i<4;i++) + { + wheel[i].setSpeed(wheelSpeed[i]); + wait(0.001); + //water[i].setSpeed(waterPower[i]); + //wait(0.001); + arm[i].setSpeed(armPower[i]); + wait(0.001); + } + led2 = can.tderror(); + /*if(can.tderror()) + { + led2 = !led2; + counter++; + if(counter==10000){ + //can.reset(); + counter=0; + } + }*/ + wait(0.01); + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Nov 30 07:26:58 2018 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/fd96258d940d \ No newline at end of file