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@7:3fcb0d1c41aa, 2015-03-07 (annotated)

Committer:
YosukeK
Date:
Sat Mar 07 15:49:55 2015 +0000
Revision:
7:3fcb0d1c41aa
Parent:
6:a3829299dfd5
Child:
8:857b3285dc79
???????????

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 6:a3829299dfd5 3 #include "def_resources.h"
YosukeK 6:a3829299dfd5 4 #include "PwmServo.h"
YosukeK 6:a3829299dfd5 5 #include "ExtendedServo.h"
YosukeK 6:a3829299dfd5 6 #include "DelayServo.h"
YosukeK 4:039a7d1ce3e9 7 #include "Roomba.h"
YosukeK 6:a3829299dfd5 8 #include "DerivedRoomba.h"
YosukeK 0:a9b204e27472 9
YosukeK 0:a9b204e27472 10 /*
YosukeK 0:a9b204e27472 11 Controller firmware for CaitSithDanger
YosukeK 0:a9b204e27472 12
YosukeK 0:a9b204e27472 13 Kyoto-Densou-An 2014
YosukeK 0:a9b204e27472 14
YosukeK 0:a9b204e27472 15 Author : yishii
YosukeK 0:a9b204e27472 16 */
YosukeK 0:a9b204e27472 17
YosukeK 0:a9b204e27472 18 #define DEBUG
YosukeK 5:4c87a8401f7c 19 #define BUFFER_SIZE 15
YosukeK 0:a9b204e27472 20
YosukeK 2:91135f19ac12 21 int oyatsuCnt = 0;
YosukeK 2:91135f19ac12 22
YosukeK 6:a3829299dfd5 23 bool initialize();
YosukeK 6:a3829299dfd5 24 int serial_gets(char *buffer, int size);
YosukeK 6:a3829299dfd5 25 void control_caitsith(char* buffer, int size);
YosukeK 6:a3829299dfd5 26 void switchNyandaBeam(bool isFiring);
YosukeK 6:a3829299dfd5 27 void controlOyatsuFeeder(bool isMoving);
YosukeK 4:039a7d1ce3e9 28
YosukeK 6:a3829299dfd5 29 SendData::SendData data;
YosukeK 4:039a7d1ce3e9 30
YosukeK 6:a3829299dfd5 31 //Ticker mTick;
YosukeK 6:a3829299dfd5 32 // Roomba* cp = new Roomba(SERIAL_TX, SERIAL_RX, LED1);//Class pointor
YosukeK 6:a3829299dfd5 33 // mTick.attach(cp, &Roomba::periodicCallback, 0.1);
YosukeK 6:a3829299dfd5 34
YosukeK 4:039a7d1ce3e9 35
YosukeK 6:a3829299dfd5 36 /*
YosukeK 6:a3829299dfd5 37 *
YosukeK 6:a3829299dfd5 38 */
YosukeK 4:039a7d1ce3e9 39
YosukeK 6:a3829299dfd5 40 int leftWheelVelocity = 0;
YosukeK 6:a3829299dfd5 41 int rightWheelVelocity = 0;
YosukeK 0:a9b204e27472 42
YosukeK 0:a9b204e27472 43 int main() {
YosukeK 6:a3829299dfd5 44 pc.baud(115200);
YosukeK 6:a3829299dfd5 45 initialize();
YosukeK 3:9ac9fdf1856e 46
YosukeK 6:a3829299dfd5 47 //SendData::SendData *data = new SendData(0, 0);
YosukeK 6:a3829299dfd5 48 //base.set(*data);
YosukeK 2:91135f19ac12 49
YosukeK 7:3fcb0d1c41aa 50 int timecount = 0;
YosukeK 6:a3829299dfd5 51 while(1) {
YosukeK 6:a3829299dfd5 52 char buffer[BUFFER_SIZE];
YosukeK 6:a3829299dfd5 53 int size = serial_gets(buffer, BUFFER_SIZE);
YosukeK 6:a3829299dfd5 54 if (size > 0) {
YosukeK 6:a3829299dfd5 55 control_caitsith(buffer, size);
YosukeK 0:a9b204e27472 56 }
YosukeK 7:3fcb0d1c41aa 57 if (timecount >= 10000) {
YosukeK 7:3fcb0d1c41aa 58 base.periodicCallback();//ルンバへ移動指示
YosukeK 7:3fcb0d1c41aa 59 timecount = 0;
YosukeK 7:3fcb0d1c41aa 60 }
YosukeK 7:3fcb0d1c41aa 61 timecount++;
YosukeK 7:3fcb0d1c41aa 62 wait_us(10);
YosukeK 0:a9b204e27472 63 }
YosukeK 0:a9b204e27472 64 }
YosukeK 0:a9b204e27472 65
YosukeK 0:a9b204e27472 66
YosukeK 6:a3829299dfd5 67 //bool
YosukeK 6:a3829299dfd5 68 void control_caitsith(char* buffer, int size) {
YosukeK 6:a3829299dfd5 69 #ifdef DEBUG
YosukeK 7:3fcb0d1c41aa 70 pc.printf("%s", buffer);
YosukeK 6:a3829299dfd5 71 #endif
YosukeK 6:a3829299dfd5 72 // コマンド判定
YosukeK 6:a3829299dfd5 73 if (strncmp(buffer, "CT", 2) == 0) {
YosukeK 6:a3829299dfd5 74 bool ret = false;
YosukeK 6:a3829299dfd5 75
YosukeK 6:a3829299dfd5 76 myled = !myled;
YosukeK 6:a3829299dfd5 77
YosukeK 6:a3829299dfd5 78 if (size == 2) {
YosukeK 6:a3829299dfd5 79 ret = true;
YosukeK 6:a3829299dfd5 80
YosukeK 6:a3829299dfd5 81 } else if (size >= 3) {
YosukeK 6:a3829299dfd5 82 switch (buffer[2]) {
YosukeK 6:a3829299dfd5 83 //緊急停止
YosukeK 6:a3829299dfd5 84 case 'Z':
YosukeK 6:a3829299dfd5 85 initialize();
YosukeK 6:a3829299dfd5 86 ret = true;
YosukeK 6:a3829299dfd5 87 break;
YosukeK 6:a3829299dfd5 88 //ニャンだろー光線
YosukeK 6:a3829299dfd5 89 case 'N':
YosukeK 6:a3829299dfd5 90 bool isFiring = ( buffer[3] == '1' ) ? true : false;
YosukeK 6:a3829299dfd5 91 switchNyandaBeam(isFiring);
YosukeK 6:a3829299dfd5 92 ret = true;//isFiring;
YosukeK 6:a3829299dfd5 93 break;
YosukeK 6:a3829299dfd5 94 //おやつベンダ
YosukeK 6:a3829299dfd5 95 case 'O':
YosukeK 6:a3829299dfd5 96 bool isMoving = ( buffer[3] == '1' ) ? true : false;
YosukeK 6:a3829299dfd5 97 controlOyatsuFeeder(isMoving);
YosukeK 6:a3829299dfd5 98 ret = true;//isMoving;
YosukeK 6:a3829299dfd5 99 break;
YosukeK 6:a3829299dfd5 100 //ルンバ移動指示
YosukeK 6:a3829299dfd5 101 case 'M'://CTM+500-500
YosukeK 6:a3829299dfd5 102 ret = true;
YosukeK 6:a3829299dfd5 103
YosukeK 6:a3829299dfd5 104 char buff[4+1];
YosukeK 6:a3829299dfd5 105 int temp;
YosukeK 6:a3829299dfd5 106 int leftWheelVelocity, rightWheelVelocity;
YosukeK 3:9ac9fdf1856e 107
YosukeK 6:a3829299dfd5 108 //pc.printf("Param:%s\n", before);
YosukeK 6:a3829299dfd5 109 strncpy(buff, buffer + 4, 4);
YosukeK 6:a3829299dfd5 110 buff[4] = '\0';
YosukeK 6:a3829299dfd5 111 pc.printf("%s\n", buff);
YosukeK 6:a3829299dfd5 112 leftWheelVelocity = atoi(buff);
YosukeK 6:a3829299dfd5 113 pc. printf("%d\n", leftWheelVelocity);
YosukeK 6:a3829299dfd5 114
YosukeK 6:a3829299dfd5 115 //printf("Param:%s\n", before);
YosukeK 6:a3829299dfd5 116 strncpy(buff, buffer + 9, 4);//(char*)buffer[3+4]
YosukeK 6:a3829299dfd5 117 buff[4] = '\0';
YosukeK 6:a3829299dfd5 118 pc. printf("%s\n", buff);
YosukeK 6:a3829299dfd5 119 rightWheelVelocity = atoi(buff);
YosukeK 6:a3829299dfd5 120 pc. printf("%d\n", rightWheelVelocity);
YosukeK 6:a3829299dfd5 121
YosukeK 6:a3829299dfd5 122 //SendData::SendData *data = new SendData(leftWheelVelocity, rightWheelVelocity);
YosukeK 6:a3829299dfd5 123
YosukeK 6:a3829299dfd5 124 data.wheelVelocity[0] = leftWheelVelocity;
YosukeK 6:a3829299dfd5 125 data.wheelVelocity[1] = rightWheelVelocity;
YosukeK 6:a3829299dfd5 126
YosukeK 6:a3829299dfd5 127 base.set(&data);
YosukeK 6:a3829299dfd5 128
YosukeK 6:a3829299dfd5 129 break;
YosukeK 6:a3829299dfd5 130
YosukeK 6:a3829299dfd5 131 case 'H':
YosukeK 6:a3829299dfd5 132 int servoAngle = -1;
YosukeK 6:a3829299dfd5 133 servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 134
YosukeK 6:a3829299dfd5 135 ret = true;
YosukeK 6:a3829299dfd5 136 if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 137 shakeHead.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 138 } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 139 nodHead.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 140 } else {
YosukeK 6:a3829299dfd5 141 ret = false;
YosukeK 6:a3829299dfd5 142 }
YosukeK 6:a3829299dfd5 143 break;
YosukeK 6:a3829299dfd5 144
YosukeK 6:a3829299dfd5 145 case 'L':
YosukeK 6:a3829299dfd5 146 servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 147
YosukeK 6:a3829299dfd5 148 ret = true;
YosukeK 6:a3829299dfd5 149 if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 150 leftShoulder.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 151 } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 152 leftArm.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 153 } else {
YosukeK 6:a3829299dfd5 154 ret = false;
YosukeK 6:a3829299dfd5 155 }
YosukeK 6:a3829299dfd5 156 break;
YosukeK 6:a3829299dfd5 157
YosukeK 6:a3829299dfd5 158 case 'R':
YosukeK 6:a3829299dfd5 159 servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 160
YosukeK 6:a3829299dfd5 161 ret = true;
YosukeK 6:a3829299dfd5 162 if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 163 rightShoulder.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 164 } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 165 rightArm.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 166 } else {
YosukeK 6:a3829299dfd5 167 ret = false;
YosukeK 6:a3829299dfd5 168 }
YosukeK 6:a3829299dfd5 169 break;
YosukeK 6:a3829299dfd5 170
YosukeK 6:a3829299dfd5 171 default:
YosukeK 6:a3829299dfd5 172 ret = false;
YosukeK 6:a3829299dfd5 173 break;
YosukeK 6:a3829299dfd5 174 }
YosukeK 6:a3829299dfd5 175 } else {
YosukeK 6:a3829299dfd5 176 ret = false;
YosukeK 6:a3829299dfd5 177 }
YosukeK 6:a3829299dfd5 178
YosukeK 6:a3829299dfd5 179 //処理結果出力
YosukeK 6:a3829299dfd5 180 if (ret) {
YosukeK 6:a3829299dfd5 181 pc.printf("\r\nOK\r\n");
YosukeK 6:a3829299dfd5 182 } else {
YosukeK 6:a3829299dfd5 183 pc.printf("\r\nNG\r\n");
YosukeK 6:a3829299dfd5 184 }
YosukeK 6:a3829299dfd5 185 }
YosukeK 6:a3829299dfd5 186 }
YosukeK 6:a3829299dfd5 187
YosukeK 6:a3829299dfd5 188
YosukeK 6:a3829299dfd5 189 //bool control_joint(char *buffer, ExtendedServo *joint1, ExtendedServo *joint2)
YosukeK 6:a3829299dfd5 190 //{
YosukeK 6:a3829299dfd5 191 // bool ret = true;
YosukeK 6:a3829299dfd5 192 // int servoAngle = -1;
YosukeK 6:a3829299dfd5 193 //
YosukeK 6:a3829299dfd5 194 // servoAngle = atoi(buffer);
YosukeK 6:a3829299dfd5 195 //
YosukeK 6:a3829299dfd5 196 // ret = true;
YosukeK 6:a3829299dfd5 197 // if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 198 // joint1->setAngle(servoAngle);
YosukeK 6:a3829299dfd5 199 // } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 200 // joint2->setAngle(servoAngle);
YosukeK 6:a3829299dfd5 201 // } else {
YosukeK 6:a3829299dfd5 202 // ret = false;
YosukeK 6:a3829299dfd5 203 // }
YosukeK 6:a3829299dfd5 204 //
YosukeK 6:a3829299dfd5 205 // return ret;
YosukeK 6:a3829299dfd5 206 //}
YosukeK 6:a3829299dfd5 207
YosukeK 6:a3829299dfd5 208
YosukeK 6:a3829299dfd5 209 /*
YosukeK 6:a3829299dfd5 210 *
YosukeK 6:a3829299dfd5 211 */
YosukeK 6:a3829299dfd5 212 bool initialize() {
YosukeK 6:a3829299dfd5 213 //pc.printf("init");
YosukeK 6:a3829299dfd5 214
YosukeK 6:a3829299dfd5 215 nyandaBeam = 0;
YosukeK 4:039a7d1ce3e9 216
YosukeK 6:a3829299dfd5 217 shakeHead.setAngle(0);
YosukeK 6:a3829299dfd5 218 nodHead.setAngle(0);
YosukeK 6:a3829299dfd5 219 leftArm.setAngle(0);
YosukeK 6:a3829299dfd5 220 leftShoulder.setAngle(0);
YosukeK 6:a3829299dfd5 221 rightArm.setAngle(0);
YosukeK 6:a3829299dfd5 222 rightShoulder.setAngle(0);
YosukeK 6:a3829299dfd5 223
YosukeK 6:a3829299dfd5 224 //Roomba
YosukeK 6:a3829299dfd5 225 SendData::SendData *data = new SendData(0, 0);
YosukeK 6:a3829299dfd5 226 base.set(data);
YosukeK 6:a3829299dfd5 227 delete data;
YosukeK 6:a3829299dfd5 228
YosukeK 6:a3829299dfd5 229 return true;
YosukeK 3:9ac9fdf1856e 230 }
YosukeK 0:a9b204e27472 231
YosukeK 3:9ac9fdf1856e 232
YosukeK 3:9ac9fdf1856e 233 /*
YosukeK 6:a3829299dfd5 234 *
YosukeK 6:a3829299dfd5 235 */
YosukeK 6:a3829299dfd5 236 void switchNyandaBeam(bool isFiring) {
YosukeK 6:a3829299dfd5 237 //pc.printf("beam");
YosukeK 6:a3829299dfd5 238
YosukeK 6:a3829299dfd5 239 if (isFiring) {
YosukeK 6:a3829299dfd5 240 nyandaBeam = 1;
YosukeK 6:a3829299dfd5 241 } else {
YosukeK 6:a3829299dfd5 242 nyandaBeam = 0;
YosukeK 6:a3829299dfd5 243 }
YosukeK 6:a3829299dfd5 244 }
YosukeK 6:a3829299dfd5 245
YosukeK 6:a3829299dfd5 246
YosukeK 6:a3829299dfd5 247 //給仕機を操作する
YosukeK 6:a3829299dfd5 248 void controlOyatsuFeeder(bool isMoving) {
YosukeK 6:a3829299dfd5 249 //pc.printf("feeder");
YosukeK 6:a3829299dfd5 250
YosukeK 6:a3829299dfd5 251 int feederDegree = 0;
YosukeK 6:a3829299dfd5 252
YosukeK 6:a3829299dfd5 253 if (isMoving) {
YosukeK 6:a3829299dfd5 254 if (oyatsuCnt < 3) {
YosukeK 6:a3829299dfd5 255 oyatsuCnt++;
YosukeK 6:a3829299dfd5 256 }
YosukeK 6:a3829299dfd5 257 //60, 90, 120
YosukeK 6:a3829299dfd5 258 feederDegree = 60 + 30 * (oyatsuCnt - 1);
YosukeK 6:a3829299dfd5 259 } else {
YosukeK 6:a3829299dfd5 260 oyatsuCnt = 0;
YosukeK 6:a3829299dfd5 261 }
YosukeK 6:a3829299dfd5 262
YosukeK 6:a3829299dfd5 263 oyatsuFeeder.setAngle(feederDegree);
YosukeK 6:a3829299dfd5 264 }
YosukeK 6:a3829299dfd5 265
YosukeK 6:a3829299dfd5 266
YosukeK 6:a3829299dfd5 267
YosukeK 6:a3829299dfd5 268 /*
YosukeK 3:9ac9fdf1856e 269 *
YosukeK 3:9ac9fdf1856e 270 */
YosukeK 3:9ac9fdf1856e 271 int serial_gets(char *buffer, int size) {
YosukeK 5:4c87a8401f7c 272 if (!pc.readable()) {
YosukeK 0:a9b204e27472 273 return 0;
YosukeK 5:4c87a8401f7c 274 }
YosukeK 0:a9b204e27472 275
YosukeK 0:a9b204e27472 276 int i = 0;
YosukeK 0:a9b204e27472 277 while (i < size - 1) {
YosukeK 0:a9b204e27472 278 if (pc.readable()) {
YosukeK 0:a9b204e27472 279 buffer[i] = pc.getc();
YosukeK 0:a9b204e27472 280 //pc.printf("%c", buffer[i]);
YosukeK 0:a9b204e27472 281
YosukeK 0:a9b204e27472 282 if (buffer[i] == '\r') {
YosukeK 7:3fcb0d1c41aa 283 //--i;
YosukeK 7:3fcb0d1c41aa 284 break;
YosukeK 0:a9b204e27472 285 }
YosukeK 0:a9b204e27472 286 if (buffer[i] == '\n') {
YosukeK 4:039a7d1ce3e9 287
YosukeK 0:a9b204e27472 288 break;
YosukeK 0:a9b204e27472 289 }
YosukeK 0:a9b204e27472 290 ++i;
YosukeK 0:a9b204e27472 291 }/* else {
YosukeK 0:a9b204e27472 292 wait_ms(1);
YosukeK 0:a9b204e27472 293 }*/
YosukeK 0:a9b204e27472 294 }
YosukeK 0:a9b204e27472 295 buffer[i] = '\0';
YosukeK 0:a9b204e27472 296 //pc.printf("%s", buffer);
YosukeK 0:a9b204e27472 297 return i;
YosukeK 0:a9b204e27472 298 }
YosukeK 4:039a7d1ce3e9 299
YosukeK 4:039a7d1ce3e9 300
YosukeK 4:039a7d1ce3e9 301
YosukeK 4:039a7d1ce3e9 302 ///*/*
YosukeK 4:039a7d1ce3e9 303 // *
YosukeK 4:039a7d1ce3e9 304 // */
YosukeK 4:039a7d1ce3e9 305 //int getMoveCommand(char opcode, char* before, char* converted)
YosukeK 4:039a7d1ce3e9 306 //{
YosukeK 4:039a7d1ce3e9 307 // char buff[10];
YosukeK 4:039a7d1ce3e9 308 // int temp;
YosukeK 4:039a7d1ce3e9 309 // int cont = 0;
YosukeK 4:039a7d1ce3e9 310 //
YosukeK 4:039a7d1ce3e9 311 // converted[cont] = opcode;
YosukeK 4:039a7d1ce3e9 312 // ++cont;
YosukeK 4:039a7d1ce3e9 313 // cont = cont + convertHexToAscString(before, 4, converted + cont);
YosukeK 4:039a7d1ce3e9 314 // cont = cont + convertHexToAscString(before + 4, 4, converted + cont);
YosukeK 4:039a7d1ce3e9 315 // converted[cont] = '\0';
YosukeK 4:039a7d1ce3e9 316 //
YosukeK 4:039a7d1ce3e9 317 //#ifdef DEBUG
YosukeK 4:039a7d1ce3e9 318 // int i = 0;
YosukeK 4:039a7d1ce3e9 319 // for (i = 0; i < cont; i++) {
YosukeK 4:039a7d1ce3e9 320 // printf("%x\n", *(converted + i));
YosukeK 4:039a7d1ce3e9 321 // }
YosukeK 4:039a7d1ce3e9 322 //#endif
YosukeK 4:039a7d1ce3e9 323 //
YosukeK 4:039a7d1ce3e9 324 // return strlen(converted);
YosukeK 4:039a7d1ce3e9 325 //}*/
YosukeK 4:039a7d1ce3e9 326 //
YosukeK 4:039a7d1ce3e9 327 ////TBD:Create Utility Class. And move under method add static
YosukeK 4:039a7d1ce3e9 328 //int convertDecToAsc(char* target, int length, int* converted) {
YosukeK 4:039a7d1ce3e9 329 // char buff[length + 1];
YosukeK 4:039a7d1ce3e9 330 // int temp;
YosukeK 4:039a7d1ce3e9 331 //
YosukeK 4:039a7d1ce3e9 332 // //strncpy(buff, before, length);
YosukeK 4:039a7d1ce3e9 333 // buff[length] = '\0';
YosukeK 4:039a7d1ce3e9 334 // //printf("ASC:%s\n", buff);
YosukeK 4:039a7d1ce3e9 335 //
YosukeK 4:039a7d1ce3e9 336 // temp = atoi(buff);
YosukeK 4:039a7d1ce3e9 337 // //printf("DEC:%d\n", temp);
YosukeK 4:039a7d1ce3e9 338 //
YosukeK 4:039a7d1ce3e9 339 // //converted = temp;
YosukeK 4:039a7d1ce3e9 340 //
YosukeK 4:039a7d1ce3e9 341 // return 0;//strlen(converted);
YosukeK 4:039a7d1ce3e9 342 //}
YosukeK 4:039a7d1ce3e9 343
YosukeK 4:039a7d1ce3e9 344 //
YosukeK 4:039a7d1ce3e9 345 ///*
YosukeK 4:039a7d1ce3e9 346 // *
YosukeK 4:039a7d1ce3e9 347 // */
YosukeK 4:039a7d1ce3e9 348 //int convertHexToAscString(char* before, int length, char* converted)
YosukeK 4:039a7d1ce3e9 349 //{
YosukeK 4:039a7d1ce3e9 350 // char buff[length + 1];
YosukeK 4:039a7d1ce3e9 351 // int temp;
YosukeK 4:039a7d1ce3e9 352 //
YosukeK 4:039a7d1ce3e9 353 // printf("Param:%s\n", before);
YosukeK 4:039a7d1ce3e9 354 //
YosukeK 4:039a7d1ce3e9 355 // strncpy(buff, before, length);
YosukeK 4:039a7d1ce3e9 356 // buff[length] = '\0';
YosukeK 4:039a7d1ce3e9 357 // printf("ASC:%s\n", buff);
YosukeK 4:039a7d1ce3e9 358 //
YosukeK 4:039a7d1ce3e9 359 // temp = atoi(buff);
YosukeK 4:039a7d1ce3e9 360 // printf("DEC:%d\n", temp);
YosukeK 4:039a7d1ce3e9 361 //
YosukeK 4:039a7d1ce3e9 362 // converted[0] = ((temp & 0xFF00) >> 8);
YosukeK 4:039a7d1ce3e9 363 // converted[1] = (temp & 0xFF);
YosukeK 4:039a7d1ce3e9 364 // converted[2] = '\0';
YosukeK 4:039a7d1ce3e9 365 // printf("HEX(MSB):%x\n", converted[0]);
YosukeK 4:039a7d1ce3e9 366 // printf("HEX(LSB):%x\n", converted[1]);
YosukeK 4:039a7d1ce3e9 367 //
YosukeK 4:039a7d1ce3e9 368 // return strlen(converted);
YosukeK 4:039a7d1ce3e9 369 //}
YosukeK 4:039a7d1ce3e9 370 //
YosukeK 6:a3829299dfd5 371
YosukeK 6:a3829299dfd5 372
YosukeK 6:a3829299dfd5 373
YosukeK 6:a3829299dfd5 374
YosukeK 6:a3829299dfd5 375
YosukeK 6:a3829299dfd5 376
YosukeK 6:a3829299dfd5 377
YosukeK 6:a3829299dfd5 378 // switch (buffer[2]) {
YosukeK 6:a3829299dfd5 379 // case 'Z':
YosukeK 6:a3829299dfd5 380 // ret = initialize();
YosukeK 6:a3829299dfd5 381 // break;
YosukeK 6:a3829299dfd5 382 //
YosukeK 6:a3829299dfd5 383 // case 'N':
YosukeK 6:a3829299dfd5 384 // bool isFiring = ( buffer[3] == '1' ) ? true : false;
YosukeK 6:a3829299dfd5 385 // switchNyandaBeam(isFiring);
YosukeK 6:a3829299dfd5 386 // ret = isFiring;
YosukeK 6:a3829299dfd5 387 // break;
YosukeK 6:a3829299dfd5 388 //
YosukeK 6:a3829299dfd5 389 // case 'M'://CTM+500-500
YosukeK 6:a3829299dfd5 390 // ret = true;
YosukeK 6:a3829299dfd5 391 //
YosukeK 6:a3829299dfd5 392 // char buff[4+1];
YosukeK 6:a3829299dfd5 393 // int temp;
YosukeK 6:a3829299dfd5 394 //
YosukeK 6:a3829299dfd5 395 // //pc.printf("Param:%s\n", before);
YosukeK 6:a3829299dfd5 396 // strncpy(buff, buffer + 3, 4);
YosukeK 6:a3829299dfd5 397 // buff[4] = '\0';
YosukeK 6:a3829299dfd5 398 // //pc.printf("ASC:%s\n", buff);
YosukeK 6:a3829299dfd5 399 // temp = atoi(buff);
YosukeK 6:a3829299dfd5 400 // e.leftWheetVelocity = temp;
YosukeK 6:a3829299dfd5 401 // //printf("Param:%s\n", before);
YosukeK 6:a3829299dfd5 402 // strncpy(buff, buffer + 7, 4);//(char*)buffer[3+4]
YosukeK 6:a3829299dfd5 403 // buff[4] = '\0';
YosukeK 6:a3829299dfd5 404 // //pc. printf("ASC:%s\n", buff);
YosukeK 6:a3829299dfd5 405 // temp = atoi(buff);
YosukeK 6:a3829299dfd5 406 // e.rightWheelVelocity = temp;
YosukeK 6:a3829299dfd5 407 //
YosukeK 6:a3829299dfd5 408 // break;
YosukeK 6:a3829299dfd5 409 //
YosukeK 6:a3829299dfd5 410 // case 'O':
YosukeK 6:a3829299dfd5 411 // ret = true;
YosukeK 6:a3829299dfd5 412 //
YosukeK 6:a3829299dfd5 413 // if (buffer[3] == '0') {
YosukeK 6:a3829299dfd5 414 // oyatsuCnt = 0;
YosukeK 6:a3829299dfd5 415 // } else if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 416 // if (oyatsuCnt < 3) {
YosukeK 6:a3829299dfd5 417 // oyatsuCnt++;
YosukeK 6:a3829299dfd5 418 // }
YosukeK 6:a3829299dfd5 419 //
YosukeK 6:a3829299dfd5 420 // switch (oyatsuCnt) {
YosukeK 6:a3829299dfd5 421 // case 0:
YosukeK 6:a3829299dfd5 422 // oyatsuFeeder.setAngle(0);
YosukeK 6:a3829299dfd5 423 // break;
YosukeK 6:a3829299dfd5 424 //
YosukeK 6:a3829299dfd5 425 // case 1:
YosukeK 6:a3829299dfd5 426 // oyatsuFeeder.setAngle(60);
YosukeK 6:a3829299dfd5 427 // break;
YosukeK 6:a3829299dfd5 428 //
YosukeK 6:a3829299dfd5 429 // case 2:
YosukeK 6:a3829299dfd5 430 // oyatsuFeeder.setAngle(90);
YosukeK 6:a3829299dfd5 431 // break;
YosukeK 6:a3829299dfd5 432 //
YosukeK 6:a3829299dfd5 433 // case 3:
YosukeK 6:a3829299dfd5 434 // oyatsuFeeder.setAngle(120);
YosukeK 6:a3829299dfd5 435 // break;
YosukeK 6:a3829299dfd5 436 //
YosukeK 6:a3829299dfd5 437 // default:
YosukeK 6:a3829299dfd5 438 // //oyatsuFeeder.setAngle(0);
YosukeK 6:a3829299dfd5 439 // break;
YosukeK 6:a3829299dfd5 440 // }
YosukeK 6:a3829299dfd5 441 // } else {
YosukeK 6:a3829299dfd5 442 // ret = false;
YosukeK 6:a3829299dfd5 443 // }
YosukeK 6:a3829299dfd5 444 //
YosukeK 6:a3829299dfd5 445 // break;
YosukeK 6:a3829299dfd5 446 //
YosukeK 6:a3829299dfd5 447 // case 'H':
YosukeK 6:a3829299dfd5 448 // int servoAngle = -1;
YosukeK 6:a3829299dfd5 449 // servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 450 //
YosukeK 6:a3829299dfd5 451 // ret = true;
YosukeK 6:a3829299dfd5 452 // if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 453 // shakeHead.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 454 // } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 455 // nodHead.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 456 // } else {
YosukeK 6:a3829299dfd5 457 // ret = false;
YosukeK 6:a3829299dfd5 458 // }
YosukeK 6:a3829299dfd5 459 // break;
YosukeK 6:a3829299dfd5 460 //
YosukeK 6:a3829299dfd5 461 // case 'L':
YosukeK 6:a3829299dfd5 462 // servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 463 //
YosukeK 6:a3829299dfd5 464 // ret = true;
YosukeK 6:a3829299dfd5 465 // if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 466 // leftShoulder.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 467 // } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 468 // leftArm.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 469 // } else {
YosukeK 6:a3829299dfd5 470 // ret = false;
YosukeK 6:a3829299dfd5 471 // }
YosukeK 6:a3829299dfd5 472 // break;
YosukeK 6:a3829299dfd5 473 //
YosukeK 6:a3829299dfd5 474 // case 'R':
YosukeK 6:a3829299dfd5 475 // servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 476 //
YosukeK 6:a3829299dfd5 477 // ret = true;
YosukeK 6:a3829299dfd5 478 // if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 479 // rightShoulder.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 480 // } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 481 // rightArm.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 482 // } else {
YosukeK 6:a3829299dfd5 483 // ret = false;
YosukeK 6:a3829299dfd5 484 // }
YosukeK 6:a3829299dfd5 485 // break;
YosukeK 6:a3829299dfd5 486 //
YosukeK 6:a3829299dfd5 487 // default:
YosukeK 6:a3829299dfd5 488 // ret = false;
YosukeK 6:a3829299dfd5 489 // break;
YosukeK 6:a3829299dfd5 490 // }
YosukeK 6:a3829299dfd5 491 // }