![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
MBed program for TR1 functions
Dependencies: mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib
main.cpp@0:1bf7e0755f40, 2021-06-02 (annotated)
- Committer:
- ftppr
- Date:
- Wed Jun 02 01:14:08 2021 +0000
- Revision:
- 0:1bf7e0755f40
- Child:
- 1:fbdbf4ac12c6
MBed programs for TR functions;
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:1bf7e0755f40 | 32 | DigitalOut lock = D15; // 電磁石 |
ftppr | 0:1bf7e0755f40 | 33 | DigitalOut back = A4; // pin 3 of faulhaber |
ftppr | 0:1bf7e0755f40 | 34 | DigitalOut eject_1 = D10; // valve 1 |
ftppr | 0:1bf7e0755f40 | 35 | DigitalOut eject_2 = D3; // 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 | 0:1bf7e0755f40 | 311 | down = 1; up = 0; |
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 | 0:1bf7e0755f40 | 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 | } |