MBed program for TR1 functions

Dependencies:   mbed Servo ros_lib_melodic

Committer:
ftppr
Date:
Wed Jun 02 13:53:04 2021 +0000
Revision:
1:fbdbf4ac12c6
Parent:
0:1bf7e0755f40

        

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ftppr 0:1bf7e0755f40 1 // Hello World to sweep a servo through its full range
ftppr 0:1bf7e0755f40 2 // ROS Connect Baud Rate
ftppr 0:1bf7e0755f40 3 #define BAUD_RATE 115200
ftppr 0:1bf7e0755f40 4
ftppr 0:1bf7e0755f40 5 #include "mbed.h"
ftppr 0:1bf7e0755f40 6 #include "Servo.h"
ftppr 0:1bf7e0755f40 7 #include <ros.h>
ftppr 0:1bf7e0755f40 8 #include <std_msgs/Int8.h>
ftppr 0:1bf7e0755f40 9 #include <std_msgs/String.h>
ftppr 0:1bf7e0755f40 10 #include <std_msgs/Int16MultiArray.h>
ftppr 0:1bf7e0755f40 11 ros::NodeHandle nh;
ftppr 0:1bf7e0755f40 12 DigitalOut myled(LED1);
ftppr 0:1bf7e0755f40 13 void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg);
ftppr 0:1bf7e0755f40 14 void msgCb_button(const std_msgs::Int8 &button_msg);
ftppr 0:1bf7e0755f40 15
ftppr 0:1bf7e0755f40 16 ros::Subscriber<std_msgs::Int16MultiArray> sub_joystick("joystick", msgCb_joystick);
ftppr 0:1bf7e0755f40 17 ros::Subscriber<std_msgs::Int8> sub_button("button", msgCb_button);
ftppr 0:1bf7e0755f40 18
ftppr 0:1bf7e0755f40 19 std_msgs::String str_msg;
ftppr 0:1bf7e0755f40 20 ros::Publisher responser("display", &str_msg);
ftppr 0:1bf7e0755f40 21
ftppr 0:1bf7e0755f40 22 char msg[200] = {0};
ftppr 0:1bf7e0755f40 23
ftppr 0:1bf7e0755f40 24 DigitalIn button(USER_BUTTON);
ftppr 0:1bf7e0755f40 25
ftppr 0:1bf7e0755f40 26 DigitalOut up = D12; // pin 1 of faulhaber
ftppr 0:1bf7e0755f40 27 DigitalOut down = A5; // pin 2 of faulhaber
ftppr 0:1bf7e0755f40 28 DigitalOut left = D2; // DC motor 1 (via L298N control pin 1)
ftppr 0:1bf7e0755f40 29 DigitalOut right = D4; // DC motor 2 (via L298N control pin 2)
ftppr 0:1bf7e0755f40 30 //AnalogOut left = A0; // DC motor 1 (via L298N control pin 1)
ftppr 0:1bf7e0755f40 31 //AnalogOut right = A1; // DC motor 2 (via L298N control pin 2)
ftppr 1:fbdbf4ac12c6 32 DigitalOut lock = D14; // 電磁石
ftppr 0:1bf7e0755f40 33 DigitalOut back = A4; // pin 3 of faulhaber
ftppr 0:1bf7e0755f40 34 DigitalOut eject_1 = D10; // valve 1
ftppr 1:fbdbf4ac12c6 35 DigitalOut eject_2 = D5; // valve 2
ftppr 0:1bf7e0755f40 36 //DigitalOut savePos = D0; savepos by back=1 & up=1 & down=1
ftppr 0:1bf7e0755f40 37 DigitalIn firing = D3;
ftppr 0:1bf7e0755f40 38
ftppr 0:1bf7e0755f40 39
ftppr 0:1bf7e0755f40 40 void itoa(int num, char *str, int radix)
ftppr 0:1bf7e0755f40 41 { /*索引表*/
ftppr 0:1bf7e0755f40 42 char index[] = "0123456789ABCDEF";
ftppr 0:1bf7e0755f40 43 unsigned unum; /*中间变量*/
ftppr 0:1bf7e0755f40 44 int i = 0, j, k;
ftppr 0:1bf7e0755f40 45 /*确定unum的值*/
ftppr 0:1bf7e0755f40 46 if (radix == 10 && num < 0) /*十进制负数*/
ftppr 0:1bf7e0755f40 47 {
ftppr 0:1bf7e0755f40 48 unum = (unsigned)-num;
ftppr 0:1bf7e0755f40 49 str[i++] = '-';
ftppr 0:1bf7e0755f40 50 }
ftppr 0:1bf7e0755f40 51 else
ftppr 0:1bf7e0755f40 52 unum = (unsigned)num; /*其他情况*/
ftppr 0:1bf7e0755f40 53 /*转换*/
ftppr 0:1bf7e0755f40 54 do
ftppr 0:1bf7e0755f40 55 {
ftppr 0:1bf7e0755f40 56 str[i++] = index[unum % (unsigned)radix];
ftppr 0:1bf7e0755f40 57 unum /= radix;
ftppr 0:1bf7e0755f40 58 } while (unum);
ftppr 0:1bf7e0755f40 59 str[i] = '\0';
ftppr 0:1bf7e0755f40 60 /*逆序*/
ftppr 0:1bf7e0755f40 61 if (str[0] == '-')
ftppr 0:1bf7e0755f40 62 k = 1; /*十进制负数*/
ftppr 0:1bf7e0755f40 63 else
ftppr 0:1bf7e0755f40 64 k = 0;
ftppr 0:1bf7e0755f40 65
ftppr 0:1bf7e0755f40 66 for (j = k; j <= (i - 1) / 2; j++)
ftppr 0:1bf7e0755f40 67 {
ftppr 0:1bf7e0755f40 68 char temp;
ftppr 0:1bf7e0755f40 69 temp = str[j];
ftppr 0:1bf7e0755f40 70 str[j] = str[i - 1 + k - j];
ftppr 0:1bf7e0755f40 71 str[i - 1 + k - j] = temp;
ftppr 0:1bf7e0755f40 72 }
ftppr 0:1bf7e0755f40 73 }
ftppr 0:1bf7e0755f40 74
ftppr 0:1bf7e0755f40 75 /*
ftppr 0:1bf7e0755f40 76 * joystick_msg: (Left/Right, x/y, value)
ftppr 0:1bf7e0755f40 77 */
ftppr 0:1bf7e0755f40 78
ftppr 0:1bf7e0755f40 79 void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg)
ftppr 0:1bf7e0755f40 80 {
ftppr 0:1bf7e0755f40 81 // myled = !myled; // blink the led
ftppr 0:1bf7e0755f40 82 memset(msg, 0, sizeof(msg));
ftppr 0:1bf7e0755f40 83 // eg. Received (0,0,-100)
ftppr 0:1bf7e0755f40 84 strcpy(msg, "Received (");
ftppr 0:1bf7e0755f40 85 int len = strlen(msg);
ftppr 0:1bf7e0755f40 86 msg[len++] = '0' + joystick_msg.data[0];
ftppr 0:1bf7e0755f40 87 msg[len++] = ',';
ftppr 0:1bf7e0755f40 88 itoa(joystick_msg.data[1], msg + len, 10);
ftppr 0:1bf7e0755f40 89 len = strlen(msg);
ftppr 0:1bf7e0755f40 90 msg[len++] = ',';
ftppr 0:1bf7e0755f40 91 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 92 len = strlen(msg);
ftppr 0:1bf7e0755f40 93 msg[len++] = ')';
ftppr 0:1bf7e0755f40 94 msg[len++] = ';';
ftppr 0:1bf7e0755f40 95 msg[len++] = ' ';
ftppr 0:1bf7e0755f40 96
ftppr 0:1bf7e0755f40 97 // Elaboration of the message
ftppr 0:1bf7e0755f40 98 // -----------L3------------
ftppr 0:1bf7e0755f40 99 // x
ftppr 0:1bf7e0755f40 100 if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 0)
ftppr 0:1bf7e0755f40 101 {
ftppr 0:1bf7e0755f40 102 strcpy(msg + len, "L3: direction is x(horizontal) and the value is ");
ftppr 0:1bf7e0755f40 103 len = strlen(msg);
ftppr 0:1bf7e0755f40 104 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 105 }
ftppr 0:1bf7e0755f40 106 // y
ftppr 0:1bf7e0755f40 107 if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 1)
ftppr 0:1bf7e0755f40 108 {
ftppr 0:1bf7e0755f40 109 strcpy(msg + len, "L3: direction is y(vertical) and the value is ");
ftppr 0:1bf7e0755f40 110 len = strlen(msg);
ftppr 0:1bf7e0755f40 111 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 112 }
ftppr 0:1bf7e0755f40 113
ftppr 0:1bf7e0755f40 114 // -----------R3------------
ftppr 0:1bf7e0755f40 115 // x
ftppr 0:1bf7e0755f40 116 if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 0)
ftppr 0:1bf7e0755f40 117 {
ftppr 0:1bf7e0755f40 118 strcpy(msg + len, "R3: direction is x(horizontal) and the value is ");
ftppr 0:1bf7e0755f40 119 len = strlen(msg);
ftppr 0:1bf7e0755f40 120 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 121 }
ftppr 0:1bf7e0755f40 122 // y
ftppr 0:1bf7e0755f40 123 if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 1)
ftppr 0:1bf7e0755f40 124 {
ftppr 0:1bf7e0755f40 125 strcpy(msg + len, "R3: direction is y(vertical) and the value is ");
ftppr 0:1bf7e0755f40 126 len = strlen(msg);
ftppr 0:1bf7e0755f40 127 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 128 }
ftppr 0:1bf7e0755f40 129
ftppr 0:1bf7e0755f40 130 // -----------L2------------
ftppr 0:1bf7e0755f40 131 if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == -1)
ftppr 0:1bf7e0755f40 132 {
ftppr 0:1bf7e0755f40 133 strcpy(msg + len, "L2: the value is ");
ftppr 0:1bf7e0755f40 134 len = strlen(msg);
ftppr 0:1bf7e0755f40 135 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 136 }
ftppr 0:1bf7e0755f40 137 // -----------R2------------
ftppr 0:1bf7e0755f40 138 if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == -1)
ftppr 0:1bf7e0755f40 139 {
ftppr 0:1bf7e0755f40 140 strcpy(msg + len, "L3: the value is ");
ftppr 0:1bf7e0755f40 141 len = strlen(msg);
ftppr 0:1bf7e0755f40 142 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 143 }
ftppr 0:1bf7e0755f40 144
ftppr 0:1bf7e0755f40 145 responser.publish(&str_msg);
ftppr 0:1bf7e0755f40 146 }
ftppr 0:1bf7e0755f40 147
ftppr 0:1bf7e0755f40 148 // button_msg: Int8
ftppr 0:1bf7e0755f40 149 void msgCb_button(const std_msgs::Int8 &button_msg)
ftppr 0:1bf7e0755f40 150 {
ftppr 0:1bf7e0755f40 151 // myled = !myled; // blink the led
ftppr 0:1bf7e0755f40 152 memset(msg, 0, sizeof(msg));
ftppr 0:1bf7e0755f40 153 strcpy(msg, "Received ");
ftppr 0:1bf7e0755f40 154 itoa(button_msg.data, msg + 9, 10);
ftppr 0:1bf7e0755f40 155 // int len = my_strlen(msg);
ftppr 0:1bf7e0755f40 156 int len = strlen(msg);
ftppr 0:1bf7e0755f40 157 msg[len++] = ';';
ftppr 0:1bf7e0755f40 158 msg[len++] = ' ';
ftppr 0:1bf7e0755f40 159
ftppr 0:1bf7e0755f40 160 switch (button_msg.data)
ftppr 0:1bf7e0755f40 161 {
ftppr 0:1bf7e0755f40 162 case 1:
ftppr 0:1bf7e0755f40 163 eject_1 = 1 - eject_1;
ftppr 0:1bf7e0755f40 164 eject_2 = 1 - eject_2;
ftppr 0:1bf7e0755f40 165 strcpy(msg + len, "x is pressed, eject_1 is ");
ftppr 0:1bf7e0755f40 166 msg[strlen(msg)] = '0' + int(eject_1);
ftppr 0:1bf7e0755f40 167 break;
ftppr 0:1bf7e0755f40 168 case -1:
ftppr 0:1bf7e0755f40 169 // eject = 0;
ftppr 0:1bf7e0755f40 170 strcpy(msg + len, "x is released, eject_2 is ");
ftppr 0:1bf7e0755f40 171 msg[strlen(msg)] = '0' + int(eject_2);
ftppr 0:1bf7e0755f40 172 break;
ftppr 0:1bf7e0755f40 173 case 2:
ftppr 0:1bf7e0755f40 174 lock = 1 - lock;
ftppr 0:1bf7e0755f40 175 strcpy(msg + len, "o is pressed, lock is: ");
ftppr 0:1bf7e0755f40 176 msg[strlen(msg)] = '0' + int(lock);
ftppr 0:1bf7e0755f40 177
ftppr 0:1bf7e0755f40 178 break;
ftppr 0:1bf7e0755f40 179 case -2:
ftppr 0:1bf7e0755f40 180 strcpy(msg + len, "o is released");
ftppr 0:1bf7e0755f40 181 break;
ftppr 0:1bf7e0755f40 182 case 3:
ftppr 0:1bf7e0755f40 183 back = 1;
ftppr 0:1bf7e0755f40 184 strcpy(msg + len, "triangle is pressed, back is ");
ftppr 0:1bf7e0755f40 185 msg[strlen(msg)] = '0' + int(back);
ftppr 0:1bf7e0755f40 186 break;
ftppr 0:1bf7e0755f40 187 case -3:
ftppr 0:1bf7e0755f40 188 back = 0;
ftppr 0:1bf7e0755f40 189 strcpy(msg + len, "triangle is released, back is ");
ftppr 0:1bf7e0755f40 190 msg[strlen(msg)] = '0' + int(back);
ftppr 0:1bf7e0755f40 191 break;
ftppr 0:1bf7e0755f40 192 case 4:
ftppr 0:1bf7e0755f40 193 strcpy(msg + len, "square is pressed");
ftppr 0:1bf7e0755f40 194 up = 1; down = 1;
ftppr 0:1bf7e0755f40 195 break;
ftppr 0:1bf7e0755f40 196 case -4:
ftppr 0:1bf7e0755f40 197 strcpy(msg + len, "square is released");
ftppr 0:1bf7e0755f40 198 up = 0; down = 0;
ftppr 0:1bf7e0755f40 199 break;
ftppr 0:1bf7e0755f40 200 case 5:
ftppr 0:1bf7e0755f40 201 down = 1;
ftppr 0:1bf7e0755f40 202 up = 0;
ftppr 0:1bf7e0755f40 203 strcpy(msg + len, "down arrow is pressed, down is ");
ftppr 0:1bf7e0755f40 204 msg[strlen(msg)] = '0' + int(down);
ftppr 0:1bf7e0755f40 205 break;
ftppr 0:1bf7e0755f40 206 case -5:
ftppr 0:1bf7e0755f40 207 down = 0;
ftppr 0:1bf7e0755f40 208 strcpy(msg + len, "down arrow is released, down is ");
ftppr 0:1bf7e0755f40 209 msg[strlen(msg)] = '0' + int(down);
ftppr 0:1bf7e0755f40 210 break;
ftppr 0:1bf7e0755f40 211 case 6:
ftppr 0:1bf7e0755f40 212 left = 0; right = 1;
ftppr 0:1bf7e0755f40 213 strcpy(msg + len, "right arrow is pressed, right is ");
ftppr 0:1bf7e0755f40 214 msg[strlen(msg)] = '0' + int(right);
ftppr 0:1bf7e0755f40 215 break;
ftppr 0:1bf7e0755f40 216 case -6:
ftppr 0:1bf7e0755f40 217 right = 0;
ftppr 0:1bf7e0755f40 218 strcpy(msg + len, "right arrow is released, right is ");
ftppr 0:1bf7e0755f40 219 msg[strlen(msg)] = '0' + int(right);
ftppr 0:1bf7e0755f40 220 break;
ftppr 0:1bf7e0755f40 221 case 7:
ftppr 0:1bf7e0755f40 222 up = 1;
ftppr 0:1bf7e0755f40 223 down = 0;
ftppr 0:1bf7e0755f40 224 strcpy(msg + len, "up arrow is pressed, up is");
ftppr 0:1bf7e0755f40 225 msg[strlen(msg)] = '0' + int(up);
ftppr 0:1bf7e0755f40 226 break;
ftppr 0:1bf7e0755f40 227 case -7:
ftppr 0:1bf7e0755f40 228 up = 0;
ftppr 0:1bf7e0755f40 229 strcpy(msg + len, "up arrow is released, up is");
ftppr 0:1bf7e0755f40 230 msg[strlen(msg)] = '0' + int(up);
ftppr 0:1bf7e0755f40 231 break;
ftppr 0:1bf7e0755f40 232 case 8:
ftppr 0:1bf7e0755f40 233 left = 1; right = 0;
ftppr 0:1bf7e0755f40 234 strcpy(msg + len, "left arrow is pressed, left is ");
ftppr 0:1bf7e0755f40 235 msg[strlen(msg)] = '0' + int(left);
ftppr 0:1bf7e0755f40 236 break;
ftppr 0:1bf7e0755f40 237 case -8:
ftppr 0:1bf7e0755f40 238 left = 0;
ftppr 0:1bf7e0755f40 239 strcpy(msg + len, "left arrow is released, left is ");
ftppr 0:1bf7e0755f40 240 msg[strlen(msg)] = '0' + int(left);
ftppr 0:1bf7e0755f40 241 break;
ftppr 0:1bf7e0755f40 242 case 9:
ftppr 0:1bf7e0755f40 243 strcpy(msg + len, "L1 is pressed, left is ");
ftppr 0:1bf7e0755f40 244 msg[strlen(msg)] = '0' + int(left);
ftppr 0:1bf7e0755f40 245 break;
ftppr 0:1bf7e0755f40 246 case -9:
ftppr 0:1bf7e0755f40 247 strcpy(msg + len, "L1 is released");
ftppr 0:1bf7e0755f40 248 break;
ftppr 0:1bf7e0755f40 249 case 10:
ftppr 0:1bf7e0755f40 250 strcpy(msg + len, "R1 is pressed");
ftppr 0:1bf7e0755f40 251 break;
ftppr 0:1bf7e0755f40 252 case -10:
ftppr 0:1bf7e0755f40 253 strcpy(msg + len, "R1 is released");
ftppr 0:1bf7e0755f40 254 break;
ftppr 0:1bf7e0755f40 255 case 11:
ftppr 0:1bf7e0755f40 256 strcpy(msg + len, "share is pressed");
ftppr 0:1bf7e0755f40 257 break;
ftppr 0:1bf7e0755f40 258 case -11:
ftppr 0:1bf7e0755f40 259 strcpy(msg + len, "share is released");
ftppr 0:1bf7e0755f40 260 break;
ftppr 0:1bf7e0755f40 261 case 12:
ftppr 0:1bf7e0755f40 262 strcpy(msg + len, "option is pressed");
ftppr 0:1bf7e0755f40 263 break;
ftppr 0:1bf7e0755f40 264 case -12:
ftppr 0:1bf7e0755f40 265 strcpy(msg + len, "option is released");
ftppr 0:1bf7e0755f40 266 break;
ftppr 0:1bf7e0755f40 267 case 13:
ftppr 0:1bf7e0755f40 268 up = 1; down = 1; back = 1;
ftppr 0:1bf7e0755f40 269 strcpy(msg + len, "playstation button is pressed, cur pos should be saved");
ftppr 0:1bf7e0755f40 270 break;
ftppr 0:1bf7e0755f40 271 case -13:
ftppr 0:1bf7e0755f40 272 up = 0; down = 0; back= 0;
ftppr 0:1bf7e0755f40 273 strcpy(msg + len, "playstation button is released");
ftppr 0:1bf7e0755f40 274 break;
ftppr 0:1bf7e0755f40 275 default:
ftppr 0:1bf7e0755f40 276 len = strlen(msg);
ftppr 0:1bf7e0755f40 277 strcpy(msg + len, "Unrecognized pattern");
ftppr 0:1bf7e0755f40 278 break;
ftppr 0:1bf7e0755f40 279 }
ftppr 0:1bf7e0755f40 280
ftppr 0:1bf7e0755f40 281 responser.publish(&str_msg);
ftppr 0:1bf7e0755f40 282 }
ftppr 0:1bf7e0755f40 283 int main()
ftppr 0:1bf7e0755f40 284 {
ftppr 0:1bf7e0755f40 285 // -> Set Baud Rate
ftppr 0:1bf7e0755f40 286 nh.getHardware()->setBaud(BAUD_RATE);
ftppr 0:1bf7e0755f40 287 nh.initNode();
ftppr 0:1bf7e0755f40 288 nh.advertise(responser);
ftppr 0:1bf7e0755f40 289 str_msg.data = msg;
ftppr 0:1bf7e0755f40 290 responser.publish(&str_msg);
ftppr 0:1bf7e0755f40 291 nh.subscribe(sub_button);
ftppr 0:1bf7e0755f40 292 nh.subscribe(sub_joystick);
ftppr 0:1bf7e0755f40 293 myled = 0;
ftppr 0:1bf7e0755f40 294 back = 0;up = 0; down = 0; lock = 0; left = 0; right = 0;
ftppr 0:1bf7e0755f40 295 eject_1 = 0; eject_2 = 1;
ftppr 0:1bf7e0755f40 296 strcpy(msg, "Byebye world.");
ftppr 0:1bf7e0755f40 297 responser.publish(&str_msg);
ftppr 0:1bf7e0755f40 298 int start = 1;
ftppr 0:1bf7e0755f40 299 firing.mode(PullDown);
ftppr 0:1bf7e0755f40 300 // Main programming
ftppr 0:1bf7e0755f40 301 while(true) {
ftppr 0:1bf7e0755f40 302 nh.spinOnce();
ftppr 0:1bf7e0755f40 303
ftppr 0:1bf7e0755f40 304 if(nh.connected())
ftppr 0:1bf7e0755f40 305 myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson
ftppr 0:1bf7e0755f40 306 else
ftppr 0:1bf7e0755f40 307 myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson
ftppr 0:1bf7e0755f40 308 if(button == 1)
ftppr 0:1bf7e0755f40 309 {
ftppr 0:1bf7e0755f40 310 myled = 0;
ftppr 1:fbdbf4ac12c6 311 down = 0; up = 1; back = 1;
ftppr 0:1bf7e0755f40 312 start = 1;
ftppr 0:1bf7e0755f40 313 while(button == 1);
ftppr 0:1bf7e0755f40 314 }
ftppr 0:1bf7e0755f40 315 if(firing.read() == 1 && start == 1)
ftppr 0:1bf7e0755f40 316 {
ftppr 0:1bf7e0755f40 317 up = 0;
ftppr 1:fbdbf4ac12c6 318 down = 1;
ftppr 0:1bf7e0755f40 319 back = 1;
ftppr 0:1bf7e0755f40 320 while(firing.read() == 1);
ftppr 0:1bf7e0755f40 321 wait(0.3);
ftppr 0:1bf7e0755f40 322 down = 0; back = 0; up = 0;
ftppr 0:1bf7e0755f40 323 start = 0;
ftppr 0:1bf7e0755f40 324
ftppr 0:1bf7e0755f40 325 }
ftppr 0:1bf7e0755f40 326 }
ftppr 0:1bf7e0755f40 327 }