Yosuke Kirihata / Mbed 2 deprecated Nucleo_CaitSith_Firmware_added_delayServo

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

Fork of Nucleo_CaitSith_Firmware by Yosuke Kirihata

main.cpp@1:5f6dd444850a, 2014-09-20 (annotated)

Committer:
YosukeK
Date:
Sat Sep 20 07:04:09 2014 +0000
Revision:
1:5f6dd444850a
Parent:
0:a9b204e27472
Child:
2:91135f19ac12
Add ExtendedServo class.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YosukeK 0:a9b204e27472 1 #include <mbed.h>
YosukeK 0:a9b204e27472 2 #include <PwmServo.h>
YosukeK 1:5f6dd444850a 3 #include <ExtendedServo.h>
YosukeK 0:a9b204e27472 4 #include <RobotArm.h>
YosukeK 0:a9b204e27472 5 #include <def_resources.h>
YosukeK 0:a9b204e27472 6 #include <Serial.h>
YosukeK 0:a9b204e27472 7
YosukeK 0:a9b204e27472 8 /*
YosukeK 0:a9b204e27472 9
YosukeK 0:a9b204e27472 10 Controller firmware for CaitSithDanger
YosukeK 0:a9b204e27472 11
YosukeK 0:a9b204e27472 12 Kyoto-Densou-An 2014
YosukeK 0:a9b204e27472 13
YosukeK 0:a9b204e27472 14 Author : yishii
YosukeK 0:a9b204e27472 15 */
YosukeK 0:a9b204e27472 16
YosukeK 0:a9b204e27472 17 #define DEBUG
YosukeK 0:a9b204e27472 18 #define BUFFER_SIZE 11
YosukeK 0:a9b204e27472 19 #define COMMAND_TOTAL 3
YosukeK 0:a9b204e27472 20
YosukeK 0:a9b204e27472 21 Serial pc(SERIAL_TX, SERIAL_RX);
YosukeK 0:a9b204e27472 22
YosukeK 0:a9b204e27472 23 DigitalOut myled(LED1);
YosukeK 0:a9b204e27472 24
YosukeK 0:a9b204e27472 25 char receive[BUFFER_SIZE];
YosukeK 0:a9b204e27472 26 //char* p_receive;
YosukeK 0:a9b204e27472 27 int recvPnt = 0;
YosukeK 0:a9b204e27472 28
YosukeK 0:a9b204e27472 29 int head0;
YosukeK 0:a9b204e27472 30 int head1;
YosukeK 0:a9b204e27472 31
YosukeK 0:a9b204e27472 32 void pc_rx ();
YosukeK 0:a9b204e27472 33
YosukeK 0:a9b204e27472 34 int serial_gets (char *buffer, int size);
YosukeK 0:a9b204e27472 35
YosukeK 0:a9b204e27472 36 int main() {
YosukeK 0:a9b204e27472 37 myled = 1;
YosukeK 0:a9b204e27472 38
YosukeK 1:5f6dd444850a 39 shakeHead.setAngle(0);
YosukeK 1:5f6dd444850a 40 nodHead.setAngle(0);
YosukeK 1:5f6dd444850a 41 leftArm.setAngle(0);
YosukeK 1:5f6dd444850a 42 leftShoulder.setAngle(0);
YosukeK 1:5f6dd444850a 43 rightArm.setAngle(0);
YosukeK 1:5f6dd444850a 44 rightShoulder.setAngle(0);
YosukeK 1:5f6dd444850a 45
YosukeK 0:a9b204e27472 46 while(1) {
YosukeK 0:a9b204e27472 47 char buffer[BUFFER_SIZE];
YosukeK 0:a9b204e27472 48
YosukeK 0:a9b204e27472 49 int size = serial_gets(buffer, BUFFER_SIZE);
YosukeK 0:a9b204e27472 50 if (size > 0) {
YosukeK 0:a9b204e27472 51 pc.printf("%s", buffer);
YosukeK 0:a9b204e27472 52
YosukeK 0:a9b204e27472 53 if (strncmp(buffer, "CT", 2) == 0) {
YosukeK 0:a9b204e27472 54
YosukeK 0:a9b204e27472 55 myled = !myled;
YosukeK 1:5f6dd444850a 56
YosukeK 1:5f6dd444850a 57 bool ret = false;
YosukeK 0:a9b204e27472 58
YosukeK 0:a9b204e27472 59 if (size == 2) {
YosukeK 1:5f6dd444850a 60 ret = true;
YosukeK 0:a9b204e27472 61 } else if (size >= 3) {
YosukeK 0:a9b204e27472 62 switch (buffer[2]) {
YosukeK 0:a9b204e27472 63 case 'O':
YosukeK 1:5f6dd444850a 64
YosukeK 1:5f6dd444850a 65 //TODO:Move OYATSU bender.
YosukeK 1:5f6dd444850a 66
YosukeK 1:5f6dd444850a 67 ret = true;
YosukeK 0:a9b204e27472 68 break;
YosukeK 0:a9b204e27472 69 case 'H':
YosukeK 0:a9b204e27472 70
YosukeK 0:a9b204e27472 71 int servoAngle = -1;
YosukeK 0:a9b204e27472 72 servoAngle = atoi((buffer+4));
YosukeK 1:5f6dd444850a 73
YosukeK 1:5f6dd444850a 74 ret = true;
YosukeK 1:5f6dd444850a 75 if (buffer[3] == '1') {
YosukeK 1:5f6dd444850a 76 shakeHead.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 77 } else if (buffer[3] == '2') {
YosukeK 1:5f6dd444850a 78 nodHead.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 79 } else {
YosukeK 1:5f6dd444850a 80 ret = false;
YosukeK 1:5f6dd444850a 81 }
YosukeK 1:5f6dd444850a 82 break;
YosukeK 0:a9b204e27472 83
YosukeK 1:5f6dd444850a 84 case 'L':
YosukeK 1:5f6dd444850a 85 servoAngle = atoi((buffer+4));
YosukeK 1:5f6dd444850a 86
YosukeK 1:5f6dd444850a 87 ret = true;
YosukeK 1:5f6dd444850a 88 if (buffer[3] == '1') {
YosukeK 1:5f6dd444850a 89 leftArm.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 90 } else if (buffer[3] == '2') {
YosukeK 1:5f6dd444850a 91 leftShoulder.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 92 } else {
YosukeK 1:5f6dd444850a 93 ret = false;
YosukeK 0:a9b204e27472 94 }
YosukeK 1:5f6dd444850a 95 break;
YosukeK 1:5f6dd444850a 96
YosukeK 1:5f6dd444850a 97 case 'R':
YosukeK 1:5f6dd444850a 98 servoAngle = atoi((buffer+4));
YosukeK 0:a9b204e27472 99
YosukeK 1:5f6dd444850a 100 ret = true;
YosukeK 0:a9b204e27472 101 if (buffer[3] == '1') {
YosukeK 1:5f6dd444850a 102 rightArm.setAngle(servoAngle);
YosukeK 0:a9b204e27472 103 } else if (buffer[3] == '2') {
YosukeK 1:5f6dd444850a 104 rightShoulder.setAngle(servoAngle);
YosukeK 0:a9b204e27472 105 } else {
YosukeK 1:5f6dd444850a 106 ret = false;
YosukeK 0:a9b204e27472 107 }
YosukeK 1:5f6dd444850a 108 break;
YosukeK 1:5f6dd444850a 109
YosukeK 1:5f6dd444850a 110 default:
YosukeK 1:5f6dd444850a 111 ret = false;
YosukeK 0:a9b204e27472 112 break;
YosukeK 0:a9b204e27472 113 }
YosukeK 0:a9b204e27472 114 }
YosukeK 0:a9b204e27472 115
YosukeK 1:5f6dd444850a 116 if (ret) {
YosukeK 1:5f6dd444850a 117 pc.printf("\r\nOK\r\n");
YosukeK 1:5f6dd444850a 118 } else {
YosukeK 1:5f6dd444850a 119 pc.printf("\r\nNG\r\n");
YosukeK 1:5f6dd444850a 120 }
YosukeK 0:a9b204e27472 121 }
YosukeK 0:a9b204e27472 122 }
YosukeK 0:a9b204e27472 123
YosukeK 0:a9b204e27472 124 }
YosukeK 0:a9b204e27472 125 }
YosukeK 0:a9b204e27472 126
YosukeK 0:a9b204e27472 127
YosukeK 0:a9b204e27472 128
YosukeK 0:a9b204e27472 129 int serial_gets (char *buffer, int size) {
YosukeK 0:a9b204e27472 130 /*if (!pc.readable()) {
YosukeK 0:a9b204e27472 131 return 0;
YosukeK 0:a9b204e27472 132 }*/
YosukeK 0:a9b204e27472 133
YosukeK 0:a9b204e27472 134 int i = 0;
YosukeK 0:a9b204e27472 135 while (i < size - 1) {
YosukeK 0:a9b204e27472 136 if (pc.readable()) {
YosukeK 0:a9b204e27472 137 buffer[i] = pc.getc();
YosukeK 0:a9b204e27472 138 //pc.printf("%c", buffer[i]);
YosukeK 0:a9b204e27472 139
YosukeK 0:a9b204e27472 140 if (buffer[i] == '\r') {
YosukeK 0:a9b204e27472 141 --i;
YosukeK 0:a9b204e27472 142 }
YosukeK 0:a9b204e27472 143 if (buffer[i] == '\n') {
YosukeK 0:a9b204e27472 144 break;
YosukeK 0:a9b204e27472 145 }
YosukeK 0:a9b204e27472 146 ++i;
YosukeK 0:a9b204e27472 147 }/* else {
YosukeK 0:a9b204e27472 148 wait_ms(1);
YosukeK 0:a9b204e27472 149 }*/
YosukeK 0:a9b204e27472 150 }
YosukeK 0:a9b204e27472 151 buffer[i] = '\0';
YosukeK 0:a9b204e27472 152 //pc.printf("%s", buffer);
YosukeK 0:a9b204e27472 153 return i;
YosukeK 0:a9b204e27472 154 }
YosukeK 0:a9b204e27472 155
YosukeK 0:a9b204e27472 156
YosukeK 0:a9b204e27472 157 //int main()
YosukeK 0:a9b204e27472 158 //{
YosukeK 0:a9b204e27472 159 // head0 = 90;
YosukeK 0:a9b204e27472 160 // head0 = head0 - 10;
YosukeK 0:a9b204e27472 161 // head1 = 90;
YosukeK 0:a9b204e27472 162 // head1 = head1 - 20;
YosukeK 0:a9b204e27472 163
YosukeK 0:a9b204e27472 164 //myled = 0;
YosukeK 0:a9b204e27472 165
YosukeK 0:a9b204e27472 166 //clear buffer
YosukeK 0:a9b204e27472 167 // memset(receive, 0, BUFFER_SIZE);
YosukeK 0:a9b204e27472 168 //receive[BUFFER_SIZE-1] = '\0';
YosukeK 0:a9b204e27472 169 //p_receive = receive;
YosukeK 0:a9b204e27472 170
YosukeK 0:a9b204e27472 171 //TODO:Create arm instance.
YosukeK 0:a9b204e27472 172
YosukeK 0:a9b204e27472 173
YosukeK 0:a9b204e27472 174 // arm1.actuate(head0, head1);
YosukeK 0:a9b204e27472 175
YosukeK 0:a9b204e27472 176
YosukeK 0:a9b204e27472 177 //pc.attach(pc_rx, Serial::RxIrq);
YosukeK 0:a9b204e27472 178
YosukeK 0:a9b204e27472 179 // while(1) {
YosukeK 0:a9b204e27472 180 // }
YosukeK 0:a9b204e27472 181
YosukeK 0:a9b204e27472 182 // int i;
YosukeK 0:a9b204e27472 183 //
YosukeK 0:a9b204e27472 184 // while(1) {
YosukeK 0:a9b204e27472 185 // for(i=0; i<180; i++) {
YosukeK 0:a9b204e27472 186 // arm1.actuate(i,i);
YosukeK 0:a9b204e27472 187 // wait(0.005);
YosukeK 0:a9b204e27472 188 // }
YosukeK 0:a9b204e27472 189 // for(i=180; i>0; i--) {
YosukeK 0:a9b204e27472 190 // arm1.actuate(i,i);
YosukeK 0:a9b204e27472 191 // wait(0.005);
YosukeK 0:a9b204e27472 192 // }
YosukeK 0:a9b204e27472 193 // }
YosukeK 0:a9b204e27472 194 //}
YosukeK 0:a9b204e27472 195
YosukeK 0:a9b204e27472 196 void pc_rx () {
YosukeK 0:a9b204e27472 197 int result = -1;
YosukeK 0:a9b204e27472 198
YosukeK 0:a9b204e27472 199 char c = pc.getc();
YosukeK 0:a9b204e27472 200 //pc.printf("%02X", (unsigned char)c);
YosukeK 0:a9b204e27472 201 pc.printf("%c", (unsigned char)c);
YosukeK 0:a9b204e27472 202
YosukeK 0:a9b204e27472 203 if (c == '\r') {
YosukeK 0:a9b204e27472 204 //Nothign to do.
YosukeK 0:a9b204e27472 205 } else if (c == '\n') {
YosukeK 0:a9b204e27472 206 receive[recvPnt] = '\0';
YosukeK 0:a9b204e27472 207 ++recvPnt;
YosukeK 0:a9b204e27472 208 } else {
YosukeK 0:a9b204e27472 209 receive[recvPnt] = c;
YosukeK 0:a9b204e27472 210 ++recvPnt;
YosukeK 0:a9b204e27472 211 }
YosukeK 0:a9b204e27472 212
YosukeK 0:a9b204e27472 213
YosukeK 0:a9b204e27472 214 if (c == '\n') {
YosukeK 0:a9b204e27472 215 if (receive[0] == 'C' && receive[1] == 'T') {
YosukeK 0:a9b204e27472 216
YosukeK 0:a9b204e27472 217 myled = 1 - myled;
YosukeK 0:a9b204e27472 218
YosukeK 0:a9b204e27472 219 if (recvPnt == 3) {
YosukeK 0:a9b204e27472 220 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 221
YosukeK 0:a9b204e27472 222 } else if (recvPnt >= 4) {
YosukeK 0:a9b204e27472 223 switch (receive[2]) {
YosukeK 0:a9b204e27472 224 case 'O':
YosukeK 0:a9b204e27472 225
YosukeK 0:a9b204e27472 226 break;
YosukeK 0:a9b204e27472 227
YosukeK 0:a9b204e27472 228 case 'H':
YosukeK 1:5f6dd444850a 229 //myled = 1;
YosukeK 0:a9b204e27472 230 int servoAngle = -1;
YosukeK 0:a9b204e27472 231 servoAngle = atoi((receive+4));
YosukeK 0:a9b204e27472 232
YosukeK 0:a9b204e27472 233 servoAngle += 90;
YosukeK 0:a9b204e27472 234 servoAngle = 180 - servoAngle;
YosukeK 0:a9b204e27472 235
YosukeK 0:a9b204e27472 236 if (servoAngle < 20) {
YosukeK 0:a9b204e27472 237 servoAngle = 20;
YosukeK 0:a9b204e27472 238 }
YosukeK 0:a9b204e27472 239 if (servoAngle > 160) {
YosukeK 0:a9b204e27472 240 servoAngle = 160;
YosukeK 0:a9b204e27472 241 }
YosukeK 0:a9b204e27472 242
YosukeK 0:a9b204e27472 243 //printf("%d\n", servoAngle);
YosukeK 0:a9b204e27472 244
YosukeK 0:a9b204e27472 245 int servoNo = -1;
YosukeK 0:a9b204e27472 246
YosukeK 0:a9b204e27472 247 if (receive[3] == '1') {
YosukeK 0:a9b204e27472 248 servoNo = 0;
YosukeK 0:a9b204e27472 249 head0 = servoAngle;
YosukeK 0:a9b204e27472 250 head0 = head0 - 10;
YosukeK 0:a9b204e27472 251 } else if (receive[3] == '2') {
YosukeK 0:a9b204e27472 252 servoNo = 1;
YosukeK 0:a9b204e27472 253 head1 = servoAngle;
YosukeK 0:a9b204e27472 254 head1 = head1 - 20;
YosukeK 0:a9b204e27472 255 } else {
YosukeK 0:a9b204e27472 256 servoNo = -1;
YosukeK 0:a9b204e27472 257 }
YosukeK 0:a9b204e27472 258 //printf("%d\n", servoNo);
YosukeK 0:a9b204e27472 259
YosukeK 0:a9b204e27472 260 //arm1.actuate(servoNo, servoAngle);
YosukeK 0:a9b204e27472 261 arm1.actuate(head0, head1);
YosukeK 0:a9b204e27472 262
YosukeK 0:a9b204e27472 263 result = 0;
YosukeK 0:a9b204e27472 264 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 265
YosukeK 0:a9b204e27472 266 break;
YosukeK 0:a9b204e27472 267
YosukeK 0:a9b204e27472 268 case 'L':
YosukeK 0:a9b204e27472 269 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 270 break;
YosukeK 0:a9b204e27472 271
YosukeK 0:a9b204e27472 272 case 'R':
YosukeK 0:a9b204e27472 273 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 274 break;
YosukeK 0:a9b204e27472 275
YosukeK 0:a9b204e27472 276 case 'N':
YosukeK 0:a9b204e27472 277
YosukeK 0:a9b204e27472 278 if (receive[3] == '1') {
YosukeK 0:a9b204e27472 279 myled = 1;
YosukeK 0:a9b204e27472 280 } else {
YosukeK 0:a9b204e27472 281 myled = 0;
YosukeK 0:a9b204e27472 282 }
YosukeK 0:a9b204e27472 283
YosukeK 0:a9b204e27472 284 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 285 break;
YosukeK 0:a9b204e27472 286
YosukeK 0:a9b204e27472 287 case 'M':
YosukeK 0:a9b204e27472 288 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 289 break;
YosukeK 0:a9b204e27472 290
YosukeK 0:a9b204e27472 291 case 'Z':
YosukeK 0:a9b204e27472 292 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 293 break;
YosukeK 0:a9b204e27472 294
YosukeK 0:a9b204e27472 295 default:
YosukeK 0:a9b204e27472 296
YosukeK 0:a9b204e27472 297 result = 1;
YosukeK 0:a9b204e27472 298 pc.printf("\r\nERROR\r\n");
YosukeK 0:a9b204e27472 299 break;
YosukeK 0:a9b204e27472 300 }
YosukeK 0:a9b204e27472 301 } else {
YosukeK 0:a9b204e27472 302 result = 1;
YosukeK 0:a9b204e27472 303 pc.printf("\r\nERROR\r\n");
YosukeK 0:a9b204e27472 304 }
YosukeK 0:a9b204e27472 305 }
YosukeK 0:a9b204e27472 306
YosukeK 0:a9b204e27472 307 //TODO:Send result the following code: CT[number]\r\n
YosukeK 0:a9b204e27472 308
YosukeK 0:a9b204e27472 309 memset(receive, 0, BUFFER_SIZE);
YosukeK 0:a9b204e27472 310 recvPnt = 0;
YosukeK 0:a9b204e27472 311 }
YosukeK 0:a9b204e27472 312
YosukeK 0:a9b204e27472 313 if (recvPnt >= BUFFER_SIZE) {
YosukeK 0:a9b204e27472 314 //myled = 0;
YosukeK 0:a9b204e27472 315 //pc.printf("NG1:Reset receive buffer.\n");
YosukeK 0:a9b204e27472 316 memset(receive, 0, BUFFER_SIZE);
YosukeK 0:a9b204e27472 317 recvPnt = 0;
YosukeK 0:a9b204e27472 318 }
YosukeK 0:a9b204e27472 319
YosukeK 0:a9b204e27472 320 }