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@5:4c87a8401f7c, 2015-01-25 (annotated)

Committer:
YosukeK
Date:
Sun Jan 25 07:45:04 2015 +0000
Revision:
5:4c87a8401f7c
Parent:
4:039a7d1ce3e9
Child:
6:a3829299dfd5
Roomba????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YosukeK 0:a9b204e27472 1 #include <mbed.h>
YosukeK 5:4c87a8401f7c 2 #include <Serial.h>
YosukeK 5:4c87a8401f7c 3
YosukeK 5:4c87a8401f7c 4 #include <def_resources.h>
YosukeK 0:a9b204e27472 5 #include <PwmServo.h>
YosukeK 1:5f6dd444850a 6 #include <ExtendedServo.h>
YosukeK 3:9ac9fdf1856e 7 #include <DelayServo.h>
YosukeK 3:9ac9fdf1856e 8 //#include <RobotArm.h>
YosukeK 4:039a7d1ce3e9 9 #include "Roomba.h"
YosukeK 4:039a7d1ce3e9 10 #include "EventArg.h"
YosukeK 4:039a7d1ce3e9 11
YosukeK 0:a9b204e27472 12
YosukeK 0:a9b204e27472 13 /*
YosukeK 0:a9b204e27472 14
YosukeK 0:a9b204e27472 15 Controller firmware for CaitSithDanger
YosukeK 0:a9b204e27472 16
YosukeK 0:a9b204e27472 17 Kyoto-Densou-An 2014
YosukeK 0:a9b204e27472 18
YosukeK 0:a9b204e27472 19 Author : yishii
YosukeK 0:a9b204e27472 20 */
YosukeK 0:a9b204e27472 21
YosukeK 0:a9b204e27472 22 #define DEBUG
YosukeK 5:4c87a8401f7c 23 #define BUFFER_SIZE 15
YosukeK 3:9ac9fdf1856e 24 //#define COMMAND_TOTAL 3
YosukeK 2:91135f19ac12 25
YosukeK 0:a9b204e27472 26
YosukeK 0:a9b204e27472 27 char receive[BUFFER_SIZE];
YosukeK 0:a9b204e27472 28 //char* p_receive;
YosukeK 3:9ac9fdf1856e 29 //int recvPnt = 0;
YosukeK 0:a9b204e27472 30
YosukeK 2:91135f19ac12 31 int oyatsuCnt = 0;
YosukeK 2:91135f19ac12 32
YosukeK 4:039a7d1ce3e9 33 EventArg e;
YosukeK 4:039a7d1ce3e9 34
YosukeK 3:9ac9fdf1856e 35 int serial_gets(char *buffer, int size);
YosukeK 0:a9b204e27472 36
YosukeK 4:039a7d1ce3e9 37 void initialize();
YosukeK 4:039a7d1ce3e9 38
YosukeK 4:039a7d1ce3e9 39 //void moveServo()
YosukeK 4:039a7d1ce3e9 40 //{
YosukeK 4:039a7d1ce3e9 41 // shakeHead.moveServo();
YosukeK 4:039a7d1ce3e9 42 // nodHead.moveServo();
YosukeK 4:039a7d1ce3e9 43 // leftArm.moveServo();
YosukeK 4:039a7d1ce3e9 44 // leftShoulder.moveServo();
YosukeK 4:039a7d1ce3e9 45 // rightArm.moveServo();
YosukeK 4:039a7d1ce3e9 46 // rightShoulder.moveServo();
YosukeK 4:039a7d1ce3e9 47 //}
YosukeK 4:039a7d1ce3e9 48
YosukeK 4:039a7d1ce3e9 49 Ticker mTick;
YosukeK 4:039a7d1ce3e9 50
YosukeK 4:039a7d1ce3e9 51 Roomba nyandaBase(SERIAL_TX, SERIAL_RX, PA_14);
YosukeK 0:a9b204e27472 52
YosukeK 0:a9b204e27472 53 int main() {
YosukeK 4:039a7d1ce3e9 54 pc.baud(115200);
YosukeK 4:039a7d1ce3e9 55 initialize();
YosukeK 4:039a7d1ce3e9 56 debugled = 1;
YosukeK 4:039a7d1ce3e9 57 //mTick.attach_us(&moveServo, 10000);
YosukeK 3:9ac9fdf1856e 58
YosukeK 4:039a7d1ce3e9 59 Roomba* cp = new Roomba(SERIAL_TX, SERIAL_RX, LED1);//Class pointor
YosukeK 5:4c87a8401f7c 60 //void (Roomba::*fp)(void) = &Roomba::serialReceiveCallback;
YosukeK 4:039a7d1ce3e9 61 //<戻り値の型> (<クラス名>::*<変数名>(<引数リスト>);
YosukeK 4:039a7d1ce3e9 62 //fp = cp->serialReceiveCallback();
YosukeK 4:039a7d1ce3e9 63 //ticker.attach(&cp, cp->serialReceiveCallback, 0.1);
YosukeK 4:039a7d1ce3e9 64
YosukeK 0:a9b204e27472 65 while(1) {
YosukeK 0:a9b204e27472 66 char buffer[BUFFER_SIZE];
YosukeK 5:4c87a8401f7c 67 // コマンド受信
YosukeK 0:a9b204e27472 68 int size = serial_gets(buffer, BUFFER_SIZE);
YosukeK 4:039a7d1ce3e9 69
YosukeK 0:a9b204e27472 70 if (size > 0) {
YosukeK 3:9ac9fdf1856e 71
YosukeK 4:039a7d1ce3e9 72 #ifdef DEBUG
YosukeK 4:039a7d1ce3e9 73 pc.printf("%s", buffer);
YosukeK 4:039a7d1ce3e9 74 #endif
YosukeK 3:9ac9fdf1856e 75 // コマンド判定
YosukeK 0:a9b204e27472 76 if (strncmp(buffer, "CT", 2) == 0) {
YosukeK 0:a9b204e27472 77
YosukeK 0:a9b204e27472 78 myled = !myled;
YosukeK 1:5f6dd444850a 79
YosukeK 1:5f6dd444850a 80 bool ret = false;
YosukeK 0:a9b204e27472 81
YosukeK 0:a9b204e27472 82 if (size == 2) {
YosukeK 1:5f6dd444850a 83 ret = true;
YosukeK 0:a9b204e27472 84 } else if (size >= 3) {
YosukeK 3:9ac9fdf1856e 85
YosukeK 5:4c87a8401f7c 86
YosukeK 4:039a7d1ce3e9 87 switch (buffer[2]) {
YosukeK 5:4c87a8401f7c 88 case 'Z':
YosukeK 3:9ac9fdf1856e 89 initialize();
YosukeK 3:9ac9fdf1856e 90
YosukeK 3:9ac9fdf1856e 91 ret = true;
YosukeK 3:9ac9fdf1856e 92 break;
YosukeK 3:9ac9fdf1856e 93
YosukeK 2:91135f19ac12 94 case 'N':
YosukeK 3:9ac9fdf1856e 95
YosukeK 2:91135f19ac12 96 ret = true;
YosukeK 2:91135f19ac12 97 if (buffer[3] == '0') {
YosukeK 2:91135f19ac12 98 nyandaLight = 0;
YosukeK 2:91135f19ac12 99 } else if (buffer[3] == '1') {
YosukeK 2:91135f19ac12 100 nyandaLight = 1;
YosukeK 2:91135f19ac12 101 } else {
YosukeK 2:91135f19ac12 102 ret = false;
YosukeK 2:91135f19ac12 103 }
YosukeK 2:91135f19ac12 104
YosukeK 2:91135f19ac12 105 break;
YosukeK 4:039a7d1ce3e9 106
YosukeK 4:039a7d1ce3e9 107 case 'M':
YosukeK 4:039a7d1ce3e9 108
YosukeK 4:039a7d1ce3e9 109 ret = true;
YosukeK 4:039a7d1ce3e9 110
YosukeK 4:039a7d1ce3e9 111 //CTM+500-500
YosukeK 4:039a7d1ce3e9 112
YosukeK 4:039a7d1ce3e9 113 char buff[4+1];
YosukeK 4:039a7d1ce3e9 114 int temp;
YosukeK 4:039a7d1ce3e9 115
YosukeK 5:4c87a8401f7c 116 //pc.printf("Param:%s\n", before);
YosukeK 4:039a7d1ce3e9 117 strncpy(buff, buffer + 3, 4);
YosukeK 4:039a7d1ce3e9 118 buff[4] = '\0';
YosukeK 5:4c87a8401f7c 119 //pc.printf("ASC:%s\n", buff);
YosukeK 4:039a7d1ce3e9 120 temp = atoi(buff);
YosukeK 5:4c87a8401f7c 121 e.leftWheetVelocity = temp;
YosukeK 4:039a7d1ce3e9 122 //printf("Param:%s\n", before);
YosukeK 5:4c87a8401f7c 123 strncpy(buff, buffer + 7, 4);//(char*)buffer[3+4]
YosukeK 4:039a7d1ce3e9 124 buff[4] = '\0';
YosukeK 5:4c87a8401f7c 125 pc. printf("ASC:%s\n", buff);
YosukeK 4:039a7d1ce3e9 126 temp = atoi(buff);
YosukeK 4:039a7d1ce3e9 127 e.rightWheelVelocity = temp;
YosukeK 5:4c87a8401f7c 128
YosukeK 4:039a7d1ce3e9 129 break;
YosukeK 2:91135f19ac12 130
YosukeK 0:a9b204e27472 131 case 'O':
YosukeK 2:91135f19ac12 132
YosukeK 1:5f6dd444850a 133 ret = true;
YosukeK 5:4c87a8401f7c 134
YosukeK 2:91135f19ac12 135 if (buffer[3] == '0') {
YosukeK 2:91135f19ac12 136 oyatsuCnt = 0;
YosukeK 2:91135f19ac12 137 oyatsuFeeder.setAngle(0);
YosukeK 2:91135f19ac12 138 } else if (buffer[3] == '1') {
YosukeK 2:91135f19ac12 139 if (oyatsuCnt <= 3) {
YosukeK 2:91135f19ac12 140 oyatsuCnt++;
YosukeK 2:91135f19ac12 141 }
YosukeK 2:91135f19ac12 142
YosukeK 2:91135f19ac12 143 switch (oyatsuCnt) {
YosukeK 2:91135f19ac12 144 //case 0:
YosukeK 2:91135f19ac12 145 // oyatusFeeder.setAngle(0);
YosukeK 2:91135f19ac12 146 //break;
YosukeK 2:91135f19ac12 147 case 1:
YosukeK 2:91135f19ac12 148 oyatsuFeeder.setAngle(60);
YosukeK 2:91135f19ac12 149 break;
YosukeK 2:91135f19ac12 150
YosukeK 2:91135f19ac12 151 case 2:
YosukeK 2:91135f19ac12 152 oyatsuFeeder.setAngle(90);
YosukeK 2:91135f19ac12 153 break;
YosukeK 2:91135f19ac12 154
YosukeK 2:91135f19ac12 155 case 3:
YosukeK 2:91135f19ac12 156 oyatsuFeeder.setAngle(120);
YosukeK 2:91135f19ac12 157 break;
YosukeK 2:91135f19ac12 158
YosukeK 2:91135f19ac12 159 default:
YosukeK 2:91135f19ac12 160 break;
YosukeK 2:91135f19ac12 161 }
YosukeK 2:91135f19ac12 162 } else {
YosukeK 2:91135f19ac12 163 ret = false;
YosukeK 2:91135f19ac12 164 }
YosukeK 2:91135f19ac12 165
YosukeK 0:a9b204e27472 166 break;
YosukeK 5:4c87a8401f7c 167
YosukeK 0:a9b204e27472 168 case 'H':
YosukeK 0:a9b204e27472 169
YosukeK 0:a9b204e27472 170 int servoAngle = -1;
YosukeK 0:a9b204e27472 171 servoAngle = atoi((buffer+4));
YosukeK 1:5f6dd444850a 172
YosukeK 1:5f6dd444850a 173 ret = true;
YosukeK 1:5f6dd444850a 174 if (buffer[3] == '1') {
YosukeK 1:5f6dd444850a 175 shakeHead.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 176 } else if (buffer[3] == '2') {
YosukeK 1:5f6dd444850a 177 nodHead.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 178 } else {
YosukeK 1:5f6dd444850a 179 ret = false;
YosukeK 1:5f6dd444850a 180 }
YosukeK 1:5f6dd444850a 181 break;
YosukeK 0:a9b204e27472 182
YosukeK 1:5f6dd444850a 183 case 'L':
YosukeK 1:5f6dd444850a 184 servoAngle = atoi((buffer+4));
YosukeK 1:5f6dd444850a 185
YosukeK 1:5f6dd444850a 186 ret = true;
YosukeK 1:5f6dd444850a 187 if (buffer[3] == '1') {
YosukeK 3:9ac9fdf1856e 188 leftShoulder.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 189 } else if (buffer[3] == '2') {
YosukeK 3:9ac9fdf1856e 190 leftArm.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 191 } else {
YosukeK 1:5f6dd444850a 192 ret = false;
YosukeK 0:a9b204e27472 193 }
YosukeK 1:5f6dd444850a 194 break;
YosukeK 1:5f6dd444850a 195
YosukeK 1:5f6dd444850a 196 case 'R':
YosukeK 1:5f6dd444850a 197 servoAngle = atoi((buffer+4));
YosukeK 0:a9b204e27472 198
YosukeK 1:5f6dd444850a 199 ret = true;
YosukeK 0:a9b204e27472 200 if (buffer[3] == '1') {
YosukeK 3:9ac9fdf1856e 201 rightShoulder.setAngle(servoAngle);
YosukeK 0:a9b204e27472 202 } else if (buffer[3] == '2') {
YosukeK 3:9ac9fdf1856e 203 rightArm.setAngle(servoAngle);
YosukeK 0:a9b204e27472 204 } else {
YosukeK 1:5f6dd444850a 205 ret = false;
YosukeK 0:a9b204e27472 206 }
YosukeK 1:5f6dd444850a 207 break;
YosukeK 1:5f6dd444850a 208
YosukeK 1:5f6dd444850a 209 default:
YosukeK 1:5f6dd444850a 210 ret = false;
YosukeK 0:a9b204e27472 211 break;
YosukeK 0:a9b204e27472 212 }
YosukeK 0:a9b204e27472 213 }
YosukeK 0:a9b204e27472 214
YosukeK 1:5f6dd444850a 215 if (ret) {
YosukeK 1:5f6dd444850a 216 pc.printf("\r\nOK\r\n");
YosukeK 1:5f6dd444850a 217 } else {
YosukeK 1:5f6dd444850a 218 pc.printf("\r\nNG\r\n");
YosukeK 1:5f6dd444850a 219 }
YosukeK 0:a9b204e27472 220 }
YosukeK 0:a9b204e27472 221 }
YosukeK 5:4c87a8401f7c 222
YosukeK 5:4c87a8401f7c 223 nyandaBase.periodicCallback(e);//ルンバへ移動指示
YosukeK 5:4c87a8401f7c 224 wait_ms(100);
YosukeK 0:a9b204e27472 225 }
YosukeK 0:a9b204e27472 226 }
YosukeK 0:a9b204e27472 227
YosukeK 0:a9b204e27472 228
YosukeK 4:039a7d1ce3e9 229 void initialize() {
YosukeK 3:9ac9fdf1856e 230 nyandaLight = 0;
YosukeK 3:9ac9fdf1856e 231
YosukeK 3:9ac9fdf1856e 232 shakeHead.setAngle(0);
YosukeK 3:9ac9fdf1856e 233 nodHead.setAngle(0);
YosukeK 3:9ac9fdf1856e 234 leftArm.setAngle(0);
YosukeK 3:9ac9fdf1856e 235 leftShoulder.setAngle(0);
YosukeK 3:9ac9fdf1856e 236 rightArm.setAngle(0);
YosukeK 3:9ac9fdf1856e 237 rightShoulder.setAngle(0);
YosukeK 4:039a7d1ce3e9 238
YosukeK 3:9ac9fdf1856e 239 }
YosukeK 0:a9b204e27472 240
YosukeK 3:9ac9fdf1856e 241
YosukeK 3:9ac9fdf1856e 242 /*
YosukeK 3:9ac9fdf1856e 243 *
YosukeK 3:9ac9fdf1856e 244 */
YosukeK 3:9ac9fdf1856e 245 int serial_gets(char *buffer, int size) {
YosukeK 5:4c87a8401f7c 246 if (!pc.readable()) {
YosukeK 0:a9b204e27472 247 return 0;
YosukeK 5:4c87a8401f7c 248 }
YosukeK 0:a9b204e27472 249
YosukeK 0:a9b204e27472 250 int i = 0;
YosukeK 0:a9b204e27472 251 while (i < size - 1) {
YosukeK 0:a9b204e27472 252 if (pc.readable()) {
YosukeK 0:a9b204e27472 253 buffer[i] = pc.getc();
YosukeK 0:a9b204e27472 254 //pc.printf("%c", buffer[i]);
YosukeK 0:a9b204e27472 255
YosukeK 0:a9b204e27472 256 if (buffer[i] == '\r') {
YosukeK 0:a9b204e27472 257 --i;
YosukeK 0:a9b204e27472 258 }
YosukeK 0:a9b204e27472 259 if (buffer[i] == '\n') {
YosukeK 4:039a7d1ce3e9 260
YosukeK 0:a9b204e27472 261 break;
YosukeK 0:a9b204e27472 262 }
YosukeK 0:a9b204e27472 263 ++i;
YosukeK 0:a9b204e27472 264 }/* else {
YosukeK 0:a9b204e27472 265 wait_ms(1);
YosukeK 0:a9b204e27472 266 }*/
YosukeK 0:a9b204e27472 267 }
YosukeK 0:a9b204e27472 268 buffer[i] = '\0';
YosukeK 0:a9b204e27472 269 //pc.printf("%s", buffer);
YosukeK 0:a9b204e27472 270 return i;
YosukeK 0:a9b204e27472 271 }
YosukeK 4:039a7d1ce3e9 272
YosukeK 4:039a7d1ce3e9 273
YosukeK 4:039a7d1ce3e9 274
YosukeK 4:039a7d1ce3e9 275 ///*/*
YosukeK 4:039a7d1ce3e9 276 // *
YosukeK 4:039a7d1ce3e9 277 // */
YosukeK 4:039a7d1ce3e9 278 //int getMoveCommand(char opcode, char* before, char* converted)
YosukeK 4:039a7d1ce3e9 279 //{
YosukeK 4:039a7d1ce3e9 280 // char buff[10];
YosukeK 4:039a7d1ce3e9 281 // int temp;
YosukeK 4:039a7d1ce3e9 282 // int cont = 0;
YosukeK 4:039a7d1ce3e9 283 //
YosukeK 4:039a7d1ce3e9 284 // converted[cont] = opcode;
YosukeK 4:039a7d1ce3e9 285 // ++cont;
YosukeK 4:039a7d1ce3e9 286 // cont = cont + convertHexToAscString(before, 4, converted + cont);
YosukeK 4:039a7d1ce3e9 287 // cont = cont + convertHexToAscString(before + 4, 4, converted + cont);
YosukeK 4:039a7d1ce3e9 288 // converted[cont] = '\0';
YosukeK 4:039a7d1ce3e9 289 //
YosukeK 4:039a7d1ce3e9 290 //#ifdef DEBUG
YosukeK 4:039a7d1ce3e9 291 // int i = 0;
YosukeK 4:039a7d1ce3e9 292 // for (i = 0; i < cont; i++) {
YosukeK 4:039a7d1ce3e9 293 // printf("%x\n", *(converted + i));
YosukeK 4:039a7d1ce3e9 294 // }
YosukeK 4:039a7d1ce3e9 295 //#endif
YosukeK 4:039a7d1ce3e9 296 //
YosukeK 4:039a7d1ce3e9 297 // return strlen(converted);
YosukeK 4:039a7d1ce3e9 298 //}*/
YosukeK 4:039a7d1ce3e9 299 //
YosukeK 4:039a7d1ce3e9 300 ////TBD:Create Utility Class. And move under method add static
YosukeK 4:039a7d1ce3e9 301 //int convertDecToAsc(char* target, int length, int* converted) {
YosukeK 4:039a7d1ce3e9 302 // char buff[length + 1];
YosukeK 4:039a7d1ce3e9 303 // int temp;
YosukeK 4:039a7d1ce3e9 304 //
YosukeK 4:039a7d1ce3e9 305 // //strncpy(buff, before, length);
YosukeK 4:039a7d1ce3e9 306 // buff[length] = '\0';
YosukeK 4:039a7d1ce3e9 307 // //printf("ASC:%s\n", buff);
YosukeK 4:039a7d1ce3e9 308 //
YosukeK 4:039a7d1ce3e9 309 // temp = atoi(buff);
YosukeK 4:039a7d1ce3e9 310 // //printf("DEC:%d\n", temp);
YosukeK 4:039a7d1ce3e9 311 //
YosukeK 4:039a7d1ce3e9 312 // //converted = temp;
YosukeK 4:039a7d1ce3e9 313 //
YosukeK 4:039a7d1ce3e9 314 // return 0;//strlen(converted);
YosukeK 4:039a7d1ce3e9 315 //}
YosukeK 4:039a7d1ce3e9 316
YosukeK 4:039a7d1ce3e9 317 //
YosukeK 4:039a7d1ce3e9 318 ///*
YosukeK 4:039a7d1ce3e9 319 // *
YosukeK 4:039a7d1ce3e9 320 // */
YosukeK 4:039a7d1ce3e9 321 //int convertHexToAscString(char* before, int length, char* converted)
YosukeK 4:039a7d1ce3e9 322 //{
YosukeK 4:039a7d1ce3e9 323 // char buff[length + 1];
YosukeK 4:039a7d1ce3e9 324 // int temp;
YosukeK 4:039a7d1ce3e9 325 //
YosukeK 4:039a7d1ce3e9 326 // printf("Param:%s\n", before);
YosukeK 4:039a7d1ce3e9 327 //
YosukeK 4:039a7d1ce3e9 328 // strncpy(buff, before, length);
YosukeK 4:039a7d1ce3e9 329 // buff[length] = '\0';
YosukeK 4:039a7d1ce3e9 330 // printf("ASC:%s\n", buff);
YosukeK 4:039a7d1ce3e9 331 //
YosukeK 4:039a7d1ce3e9 332 // temp = atoi(buff);
YosukeK 4:039a7d1ce3e9 333 // printf("DEC:%d\n", temp);
YosukeK 4:039a7d1ce3e9 334 //
YosukeK 4:039a7d1ce3e9 335 // converted[0] = ((temp & 0xFF00) >> 8);
YosukeK 4:039a7d1ce3e9 336 // converted[1] = (temp & 0xFF);
YosukeK 4:039a7d1ce3e9 337 // converted[2] = '\0';
YosukeK 4:039a7d1ce3e9 338 // printf("HEX(MSB):%x\n", converted[0]);
YosukeK 4:039a7d1ce3e9 339 // printf("HEX(LSB):%x\n", converted[1]);
YosukeK 4:039a7d1ce3e9 340 //
YosukeK 4:039a7d1ce3e9 341 // return strlen(converted);
YosukeK 4:039a7d1ce3e9 342 //}
YosukeK 4:039a7d1ce3e9 343 //