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@2:91135f19ac12, 2014-09-23 (annotated)

Committer:
YosukeK
Date:
Tue Sep 23 04:45:25 2014 +0000
Revision:
2:91135f19ac12
Parent:
1:5f6dd444850a
Child:
3:9ac9fdf1856e
Add function for operating oyatsu feeder.

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 2:91135f19ac12 21
YosukeK 0:a9b204e27472 22
YosukeK 0:a9b204e27472 23 char receive[BUFFER_SIZE];
YosukeK 0:a9b204e27472 24 //char* p_receive;
YosukeK 0:a9b204e27472 25 int recvPnt = 0;
YosukeK 0:a9b204e27472 26
YosukeK 0:a9b204e27472 27 int head0;
YosukeK 0:a9b204e27472 28 int head1;
YosukeK 0:a9b204e27472 29
YosukeK 2:91135f19ac12 30 int oyatsuCnt = 0;
YosukeK 2:91135f19ac12 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 2:91135f19ac12 63 case 'N':
YosukeK 2:91135f19ac12 64 ret = true;
YosukeK 2:91135f19ac12 65 if (buffer[3] == '0') {
YosukeK 2:91135f19ac12 66 nyandaLight = 0;
YosukeK 2:91135f19ac12 67 } else if (buffer[3] == '1') {
YosukeK 2:91135f19ac12 68 nyandaLight = 1;
YosukeK 2:91135f19ac12 69 } else {
YosukeK 2:91135f19ac12 70 ret = false;
YosukeK 2:91135f19ac12 71 }
YosukeK 2:91135f19ac12 72
YosukeK 2:91135f19ac12 73 break;
YosukeK 2:91135f19ac12 74
YosukeK 0:a9b204e27472 75 case 'O':
YosukeK 2:91135f19ac12 76
YosukeK 1:5f6dd444850a 77 ret = true;
YosukeK 2:91135f19ac12 78 if (buffer[3] == '0') {
YosukeK 2:91135f19ac12 79 oyatsuCnt = 0;
YosukeK 2:91135f19ac12 80 oyatsuFeeder.setAngle(0);
YosukeK 2:91135f19ac12 81 } else if (buffer[3] == '1') {
YosukeK 2:91135f19ac12 82 if (oyatsuCnt <= 3) {
YosukeK 2:91135f19ac12 83 oyatsuCnt++;
YosukeK 2:91135f19ac12 84 }
YosukeK 2:91135f19ac12 85
YosukeK 2:91135f19ac12 86 switch (oyatsuCnt) {
YosukeK 2:91135f19ac12 87 //case 0:
YosukeK 2:91135f19ac12 88 // oyatusFeeder.setAngle(0);
YosukeK 2:91135f19ac12 89 //break;
YosukeK 2:91135f19ac12 90 case 1:
YosukeK 2:91135f19ac12 91 oyatsuFeeder.setAngle(60);
YosukeK 2:91135f19ac12 92 break;
YosukeK 2:91135f19ac12 93
YosukeK 2:91135f19ac12 94 case 2:
YosukeK 2:91135f19ac12 95 oyatsuFeeder.setAngle(90);
YosukeK 2:91135f19ac12 96 break;
YosukeK 2:91135f19ac12 97
YosukeK 2:91135f19ac12 98 case 3:
YosukeK 2:91135f19ac12 99 oyatsuFeeder.setAngle(120);
YosukeK 2:91135f19ac12 100 break;
YosukeK 2:91135f19ac12 101
YosukeK 2:91135f19ac12 102 default:
YosukeK 2:91135f19ac12 103 break;
YosukeK 2:91135f19ac12 104 }
YosukeK 2:91135f19ac12 105 } else {
YosukeK 2:91135f19ac12 106 ret = false;
YosukeK 2:91135f19ac12 107 }
YosukeK 2:91135f19ac12 108
YosukeK 0:a9b204e27472 109 break;
YosukeK 0:a9b204e27472 110 case 'H':
YosukeK 0:a9b204e27472 111
YosukeK 0:a9b204e27472 112 int servoAngle = -1;
YosukeK 0:a9b204e27472 113 servoAngle = atoi((buffer+4));
YosukeK 1:5f6dd444850a 114
YosukeK 1:5f6dd444850a 115 ret = true;
YosukeK 1:5f6dd444850a 116 if (buffer[3] == '1') {
YosukeK 1:5f6dd444850a 117 shakeHead.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 118 } else if (buffer[3] == '2') {
YosukeK 1:5f6dd444850a 119 nodHead.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 120 } else {
YosukeK 1:5f6dd444850a 121 ret = false;
YosukeK 1:5f6dd444850a 122 }
YosukeK 1:5f6dd444850a 123 break;
YosukeK 0:a9b204e27472 124
YosukeK 1:5f6dd444850a 125 case 'L':
YosukeK 1:5f6dd444850a 126 servoAngle = atoi((buffer+4));
YosukeK 1:5f6dd444850a 127
YosukeK 1:5f6dd444850a 128 ret = true;
YosukeK 1:5f6dd444850a 129 if (buffer[3] == '1') {
YosukeK 1:5f6dd444850a 130 leftArm.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 131 } else if (buffer[3] == '2') {
YosukeK 1:5f6dd444850a 132 leftShoulder.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 133 } else {
YosukeK 1:5f6dd444850a 134 ret = false;
YosukeK 0:a9b204e27472 135 }
YosukeK 1:5f6dd444850a 136 break;
YosukeK 1:5f6dd444850a 137
YosukeK 1:5f6dd444850a 138 case 'R':
YosukeK 1:5f6dd444850a 139 servoAngle = atoi((buffer+4));
YosukeK 0:a9b204e27472 140
YosukeK 1:5f6dd444850a 141 ret = true;
YosukeK 0:a9b204e27472 142 if (buffer[3] == '1') {
YosukeK 1:5f6dd444850a 143 rightArm.setAngle(servoAngle);
YosukeK 0:a9b204e27472 144 } else if (buffer[3] == '2') {
YosukeK 1:5f6dd444850a 145 rightShoulder.setAngle(servoAngle);
YosukeK 0:a9b204e27472 146 } else {
YosukeK 1:5f6dd444850a 147 ret = false;
YosukeK 0:a9b204e27472 148 }
YosukeK 1:5f6dd444850a 149 break;
YosukeK 1:5f6dd444850a 150
YosukeK 1:5f6dd444850a 151 default:
YosukeK 1:5f6dd444850a 152 ret = false;
YosukeK 0:a9b204e27472 153 break;
YosukeK 0:a9b204e27472 154 }
YosukeK 0:a9b204e27472 155 }
YosukeK 0:a9b204e27472 156
YosukeK 1:5f6dd444850a 157 if (ret) {
YosukeK 1:5f6dd444850a 158 pc.printf("\r\nOK\r\n");
YosukeK 1:5f6dd444850a 159 } else {
YosukeK 1:5f6dd444850a 160 pc.printf("\r\nNG\r\n");
YosukeK 1:5f6dd444850a 161 }
YosukeK 0:a9b204e27472 162 }
YosukeK 0:a9b204e27472 163 }
YosukeK 0:a9b204e27472 164
YosukeK 0:a9b204e27472 165 }
YosukeK 0:a9b204e27472 166 }
YosukeK 0:a9b204e27472 167
YosukeK 0:a9b204e27472 168
YosukeK 0:a9b204e27472 169
YosukeK 0:a9b204e27472 170 int serial_gets (char *buffer, int size) {
YosukeK 0:a9b204e27472 171 /*if (!pc.readable()) {
YosukeK 0:a9b204e27472 172 return 0;
YosukeK 0:a9b204e27472 173 }*/
YosukeK 0:a9b204e27472 174
YosukeK 0:a9b204e27472 175 int i = 0;
YosukeK 0:a9b204e27472 176 while (i < size - 1) {
YosukeK 0:a9b204e27472 177 if (pc.readable()) {
YosukeK 0:a9b204e27472 178 buffer[i] = pc.getc();
YosukeK 0:a9b204e27472 179 //pc.printf("%c", buffer[i]);
YosukeK 0:a9b204e27472 180
YosukeK 0:a9b204e27472 181 if (buffer[i] == '\r') {
YosukeK 0:a9b204e27472 182 --i;
YosukeK 0:a9b204e27472 183 }
YosukeK 0:a9b204e27472 184 if (buffer[i] == '\n') {
YosukeK 0:a9b204e27472 185 break;
YosukeK 0:a9b204e27472 186 }
YosukeK 0:a9b204e27472 187 ++i;
YosukeK 0:a9b204e27472 188 }/* else {
YosukeK 0:a9b204e27472 189 wait_ms(1);
YosukeK 0:a9b204e27472 190 }*/
YosukeK 0:a9b204e27472 191 }
YosukeK 0:a9b204e27472 192 buffer[i] = '\0';
YosukeK 0:a9b204e27472 193 //pc.printf("%s", buffer);
YosukeK 0:a9b204e27472 194 return i;
YosukeK 0:a9b204e27472 195 }
YosukeK 0:a9b204e27472 196
YosukeK 0:a9b204e27472 197
YosukeK 0:a9b204e27472 198 //int main()
YosukeK 0:a9b204e27472 199 //{
YosukeK 0:a9b204e27472 200 // head0 = 90;
YosukeK 0:a9b204e27472 201 // head0 = head0 - 10;
YosukeK 0:a9b204e27472 202 // head1 = 90;
YosukeK 0:a9b204e27472 203 // head1 = head1 - 20;
YosukeK 0:a9b204e27472 204
YosukeK 0:a9b204e27472 205 //myled = 0;
YosukeK 0:a9b204e27472 206
YosukeK 0:a9b204e27472 207 //clear buffer
YosukeK 0:a9b204e27472 208 // memset(receive, 0, BUFFER_SIZE);
YosukeK 0:a9b204e27472 209 //receive[BUFFER_SIZE-1] = '\0';
YosukeK 0:a9b204e27472 210 //p_receive = receive;
YosukeK 0:a9b204e27472 211
YosukeK 0:a9b204e27472 212 //TODO:Create arm instance.
YosukeK 0:a9b204e27472 213
YosukeK 0:a9b204e27472 214
YosukeK 0:a9b204e27472 215 // arm1.actuate(head0, head1);
YosukeK 0:a9b204e27472 216
YosukeK 0:a9b204e27472 217
YosukeK 0:a9b204e27472 218 //pc.attach(pc_rx, Serial::RxIrq);
YosukeK 0:a9b204e27472 219
YosukeK 0:a9b204e27472 220 // while(1) {
YosukeK 0:a9b204e27472 221 // }
YosukeK 0:a9b204e27472 222
YosukeK 0:a9b204e27472 223 // int i;
YosukeK 0:a9b204e27472 224 //
YosukeK 0:a9b204e27472 225 // while(1) {
YosukeK 0:a9b204e27472 226 // for(i=0; i<180; i++) {
YosukeK 0:a9b204e27472 227 // arm1.actuate(i,i);
YosukeK 0:a9b204e27472 228 // wait(0.005);
YosukeK 0:a9b204e27472 229 // }
YosukeK 0:a9b204e27472 230 // for(i=180; i>0; i--) {
YosukeK 0:a9b204e27472 231 // arm1.actuate(i,i);
YosukeK 0:a9b204e27472 232 // wait(0.005);
YosukeK 0:a9b204e27472 233 // }
YosukeK 0:a9b204e27472 234 // }
YosukeK 0:a9b204e27472 235 //}
YosukeK 0:a9b204e27472 236
YosukeK 0:a9b204e27472 237 void pc_rx () {
YosukeK 0:a9b204e27472 238 int result = -1;
YosukeK 0:a9b204e27472 239
YosukeK 0:a9b204e27472 240 char c = pc.getc();
YosukeK 0:a9b204e27472 241 //pc.printf("%02X", (unsigned char)c);
YosukeK 0:a9b204e27472 242 pc.printf("%c", (unsigned char)c);
YosukeK 0:a9b204e27472 243
YosukeK 0:a9b204e27472 244 if (c == '\r') {
YosukeK 0:a9b204e27472 245 //Nothign to do.
YosukeK 0:a9b204e27472 246 } else if (c == '\n') {
YosukeK 0:a9b204e27472 247 receive[recvPnt] = '\0';
YosukeK 0:a9b204e27472 248 ++recvPnt;
YosukeK 0:a9b204e27472 249 } else {
YosukeK 0:a9b204e27472 250 receive[recvPnt] = c;
YosukeK 0:a9b204e27472 251 ++recvPnt;
YosukeK 0:a9b204e27472 252 }
YosukeK 0:a9b204e27472 253
YosukeK 0:a9b204e27472 254
YosukeK 0:a9b204e27472 255 if (c == '\n') {
YosukeK 0:a9b204e27472 256 if (receive[0] == 'C' && receive[1] == 'T') {
YosukeK 0:a9b204e27472 257
YosukeK 0:a9b204e27472 258 myled = 1 - myled;
YosukeK 0:a9b204e27472 259
YosukeK 0:a9b204e27472 260 if (recvPnt == 3) {
YosukeK 0:a9b204e27472 261 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 262
YosukeK 0:a9b204e27472 263 } else if (recvPnt >= 4) {
YosukeK 0:a9b204e27472 264 switch (receive[2]) {
YosukeK 0:a9b204e27472 265 case 'O':
YosukeK 0:a9b204e27472 266
YosukeK 0:a9b204e27472 267 break;
YosukeK 0:a9b204e27472 268
YosukeK 0:a9b204e27472 269 case 'H':
YosukeK 1:5f6dd444850a 270 //myled = 1;
YosukeK 0:a9b204e27472 271 int servoAngle = -1;
YosukeK 0:a9b204e27472 272 servoAngle = atoi((receive+4));
YosukeK 0:a9b204e27472 273
YosukeK 0:a9b204e27472 274 servoAngle += 90;
YosukeK 0:a9b204e27472 275 servoAngle = 180 - servoAngle;
YosukeK 0:a9b204e27472 276
YosukeK 0:a9b204e27472 277 if (servoAngle < 20) {
YosukeK 0:a9b204e27472 278 servoAngle = 20;
YosukeK 0:a9b204e27472 279 }
YosukeK 0:a9b204e27472 280 if (servoAngle > 160) {
YosukeK 0:a9b204e27472 281 servoAngle = 160;
YosukeK 0:a9b204e27472 282 }
YosukeK 0:a9b204e27472 283
YosukeK 0:a9b204e27472 284 //printf("%d\n", servoAngle);
YosukeK 0:a9b204e27472 285
YosukeK 0:a9b204e27472 286 int servoNo = -1;
YosukeK 0:a9b204e27472 287
YosukeK 0:a9b204e27472 288 if (receive[3] == '1') {
YosukeK 0:a9b204e27472 289 servoNo = 0;
YosukeK 0:a9b204e27472 290 head0 = servoAngle;
YosukeK 0:a9b204e27472 291 head0 = head0 - 10;
YosukeK 0:a9b204e27472 292 } else if (receive[3] == '2') {
YosukeK 0:a9b204e27472 293 servoNo = 1;
YosukeK 0:a9b204e27472 294 head1 = servoAngle;
YosukeK 0:a9b204e27472 295 head1 = head1 - 20;
YosukeK 0:a9b204e27472 296 } else {
YosukeK 0:a9b204e27472 297 servoNo = -1;
YosukeK 0:a9b204e27472 298 }
YosukeK 0:a9b204e27472 299 //printf("%d\n", servoNo);
YosukeK 0:a9b204e27472 300
YosukeK 0:a9b204e27472 301 //arm1.actuate(servoNo, servoAngle);
YosukeK 0:a9b204e27472 302 arm1.actuate(head0, head1);
YosukeK 0:a9b204e27472 303
YosukeK 0:a9b204e27472 304 result = 0;
YosukeK 0:a9b204e27472 305 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 306
YosukeK 0:a9b204e27472 307 break;
YosukeK 0:a9b204e27472 308
YosukeK 0:a9b204e27472 309 case 'L':
YosukeK 0:a9b204e27472 310 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 311 break;
YosukeK 0:a9b204e27472 312
YosukeK 0:a9b204e27472 313 case 'R':
YosukeK 0:a9b204e27472 314 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 315 break;
YosukeK 0:a9b204e27472 316
YosukeK 0:a9b204e27472 317 case 'N':
YosukeK 0:a9b204e27472 318
YosukeK 0:a9b204e27472 319 if (receive[3] == '1') {
YosukeK 0:a9b204e27472 320 myled = 1;
YosukeK 0:a9b204e27472 321 } else {
YosukeK 0:a9b204e27472 322 myled = 0;
YosukeK 0:a9b204e27472 323 }
YosukeK 0:a9b204e27472 324
YosukeK 0:a9b204e27472 325 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 326 break;
YosukeK 0:a9b204e27472 327
YosukeK 0:a9b204e27472 328 case 'M':
YosukeK 0:a9b204e27472 329 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 330 break;
YosukeK 0:a9b204e27472 331
YosukeK 0:a9b204e27472 332 case 'Z':
YosukeK 0:a9b204e27472 333 pc.printf("\r\nOK\r\n");
YosukeK 0:a9b204e27472 334 break;
YosukeK 0:a9b204e27472 335
YosukeK 0:a9b204e27472 336 default:
YosukeK 0:a9b204e27472 337
YosukeK 0:a9b204e27472 338 result = 1;
YosukeK 0:a9b204e27472 339 pc.printf("\r\nERROR\r\n");
YosukeK 0:a9b204e27472 340 break;
YosukeK 0:a9b204e27472 341 }
YosukeK 0:a9b204e27472 342 } else {
YosukeK 0:a9b204e27472 343 result = 1;
YosukeK 0:a9b204e27472 344 pc.printf("\r\nERROR\r\n");
YosukeK 0:a9b204e27472 345 }
YosukeK 0:a9b204e27472 346 }
YosukeK 0:a9b204e27472 347
YosukeK 0:a9b204e27472 348 //TODO:Send result the following code: CT[number]\r\n
YosukeK 0:a9b204e27472 349
YosukeK 0:a9b204e27472 350 memset(receive, 0, BUFFER_SIZE);
YosukeK 0:a9b204e27472 351 recvPnt = 0;
YosukeK 0:a9b204e27472 352 }
YosukeK 0:a9b204e27472 353
YosukeK 0:a9b204e27472 354 if (recvPnt >= BUFFER_SIZE) {
YosukeK 0:a9b204e27472 355 //myled = 0;
YosukeK 0:a9b204e27472 356 //pc.printf("NG1:Reset receive buffer.\n");
YosukeK 0:a9b204e27472 357 memset(receive, 0, BUFFER_SIZE);
YosukeK 0:a9b204e27472 358 recvPnt = 0;
YosukeK 0:a9b204e27472 359 }
YosukeK 0:a9b204e27472 360
YosukeK 0:a9b204e27472 361 }