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@0:a9b204e27472, 2014-09-11 (annotated)

Committer:
YosukeK
Date:
Thu Sep 11 13:45:07 2014 +0000
Revision:
0:a9b204e27472
Child:
1:5f6dd444850a
Modified receive implementation interrupt to uninterrupt.

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