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@8:857b3285dc79, 2015-03-07 (annotated)

Committer:
YosukeK
Date:
Sat Mar 07 17:46:56 2015 +0000
Revision:
8:857b3285dc79
Parent:
7:3fcb0d1c41aa 
???????????????????????????????????????????????

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 8:857b3285dc79 103
YosukeK 8:857b3285dc79 104 int left = 0;
YosukeK 8:857b3285dc79 105 int right = 0;
YosukeK 8:857b3285dc79 106
YosukeK 8:857b3285dc79 107 int length = strlen(buffer);
YosukeK 8:857b3285dc79 108 pc.printf("length:%d\n", length);
YosukeK 8:857b3285dc79 109 if(buffer[3] == 'L') {
YosukeK 8:857b3285dc79 110 left = atoi(buffer + 4);
YosukeK 8:857b3285dc79 111 pc.printf("left:%d\n", left);
YosukeK 6:a3829299dfd5 112
YosukeK 8:857b3285dc79 113 int i = 4;
YosukeK 8:857b3285dc79 114 while (i < length) {
YosukeK 8:857b3285dc79 115 i++;
YosukeK 8:857b3285dc79 116 if (buffer[i] == 'R') {
YosukeK 8:857b3285dc79 117 pc.printf("find R index:%d\n", i);
YosukeK 8:857b3285dc79 118 break;
YosukeK 8:857b3285dc79 119 }
YosukeK 8:857b3285dc79 120 }
YosukeK 8:857b3285dc79 121 int right = atoi(buffer + i+1);
YosukeK 8:857b3285dc79 122 pc.printf("right:%d\n", right);
YosukeK 8:857b3285dc79 123
YosukeK 8:857b3285dc79 124 } else {
YosukeK 8:857b3285dc79 125 ret = false;
YosukeK 8:857b3285dc79 126 break;
YosukeK 8:857b3285dc79 127 }
YosukeK 8:857b3285dc79 128
YosukeK 8:857b3285dc79 129 //
YosukeK 8:857b3285dc79 130 // char buff[4+1];
YosukeK 8:857b3285dc79 131 // int temp;
YosukeK 8:857b3285dc79 132 // int leftWheelVelocity, rightWheelVelocity;
YosukeK 8:857b3285dc79 133 //
YosukeK 8:857b3285dc79 134 // while (buffer + i == 'R') {
YosukeK 8:857b3285dc79 135 // i++;
YosukeK 8:857b3285dc79 136 // }
YosukeK 8:857b3285dc79 137 //
YosukeK 8:857b3285dc79 138 // //pc.printf("Param:%s\n", before);
YosukeK 8:857b3285dc79 139 // strncpy(buff, buffer + 4, 4);
YosukeK 8:857b3285dc79 140 // buff[4] = '\0';
YosukeK 8:857b3285dc79 141 // pc.printf("%s\n", buff);
YosukeK 8:857b3285dc79 142 // leftWheelVelocity = atoi(buff);
YosukeK 8:857b3285dc79 143 // pc. printf("%d\n", leftWheelVelocity);
YosukeK 8:857b3285dc79 144 //
YosukeK 8:857b3285dc79 145 // //printf("Param:%s\n", before);
YosukeK 8:857b3285dc79 146 // strncpy(buff, buffer + 9, 4);//(char*)buffer[3+4]
YosukeK 8:857b3285dc79 147 // buff[4] = '\0';
YosukeK 8:857b3285dc79 148 // pc. printf("%s\n", buff);
YosukeK 8:857b3285dc79 149 // rightWheelVelocity = atoi(buff);
YosukeK 8:857b3285dc79 150 // pc. printf("%d\n", rightWheelVelocity);
YosukeK 6:a3829299dfd5 151
YosukeK 6:a3829299dfd5 152 //SendData::SendData *data = new SendData(leftWheelVelocity, rightWheelVelocity);
YosukeK 6:a3829299dfd5 153
YosukeK 8:857b3285dc79 154 data.wheelVelocity[0] = left;//leftWheelVelocity;
YosukeK 8:857b3285dc79 155 data.wheelVelocity[1] = right;//rightWheelVelocity;
YosukeK 8:857b3285dc79 156
YosukeK 8:857b3285dc79 157 pc. printf("%d, %d\n", data.wheelVelocity[0], data.wheelVelocity[1]);
YosukeK 8:857b3285dc79 158
YosukeK 6:a3829299dfd5 159
YosukeK 6:a3829299dfd5 160 base.set(&data);
YosukeK 6:a3829299dfd5 161
YosukeK 6:a3829299dfd5 162 break;
YosukeK 6:a3829299dfd5 163
YosukeK 6:a3829299dfd5 164 case 'H':
YosukeK 6:a3829299dfd5 165 int servoAngle = -1;
YosukeK 6:a3829299dfd5 166 servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 167
YosukeK 6:a3829299dfd5 168 ret = true;
YosukeK 6:a3829299dfd5 169 if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 170 shakeHead.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 171 } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 172 nodHead.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 173 } else {
YosukeK 6:a3829299dfd5 174 ret = false;
YosukeK 6:a3829299dfd5 175 }
YosukeK 6:a3829299dfd5 176 break;
YosukeK 6:a3829299dfd5 177
YosukeK 6:a3829299dfd5 178 case 'L':
YosukeK 6:a3829299dfd5 179 servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 180
YosukeK 6:a3829299dfd5 181 ret = true;
YosukeK 6:a3829299dfd5 182 if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 183 leftShoulder.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 184 } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 185 leftArm.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 186 } else {
YosukeK 6:a3829299dfd5 187 ret = false;
YosukeK 6:a3829299dfd5 188 }
YosukeK 6:a3829299dfd5 189 break;
YosukeK 6:a3829299dfd5 190
YosukeK 6:a3829299dfd5 191 case 'R':
YosukeK 6:a3829299dfd5 192 servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 193
YosukeK 6:a3829299dfd5 194 ret = true;
YosukeK 6:a3829299dfd5 195 if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 196 rightShoulder.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 197 } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 198 rightArm.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 199 } else {
YosukeK 6:a3829299dfd5 200 ret = false;
YosukeK 6:a3829299dfd5 201 }
YosukeK 6:a3829299dfd5 202 break;
YosukeK 6:a3829299dfd5 203
YosukeK 6:a3829299dfd5 204 default:
YosukeK 6:a3829299dfd5 205 ret = false;
YosukeK 6:a3829299dfd5 206 break;
YosukeK 6:a3829299dfd5 207 }
YosukeK 6:a3829299dfd5 208 } else {
YosukeK 6:a3829299dfd5 209 ret = false;
YosukeK 6:a3829299dfd5 210 }
YosukeK 6:a3829299dfd5 211
YosukeK 6:a3829299dfd5 212 //処理結果出力
YosukeK 6:a3829299dfd5 213 if (ret) {
YosukeK 6:a3829299dfd5 214 pc.printf("\r\nOK\r\n");
YosukeK 6:a3829299dfd5 215 } else {
YosukeK 6:a3829299dfd5 216 pc.printf("\r\nNG\r\n");
YosukeK 6:a3829299dfd5 217 }
YosukeK 6:a3829299dfd5 218 }
YosukeK 6:a3829299dfd5 219 }
YosukeK 6:a3829299dfd5 220
YosukeK 6:a3829299dfd5 221
YosukeK 6:a3829299dfd5 222 //bool control_joint(char *buffer, ExtendedServo *joint1, ExtendedServo *joint2)
YosukeK 6:a3829299dfd5 223 //{
YosukeK 6:a3829299dfd5 224 // bool ret = true;
YosukeK 6:a3829299dfd5 225 // int servoAngle = -1;
YosukeK 6:a3829299dfd5 226 //
YosukeK 6:a3829299dfd5 227 // servoAngle = atoi(buffer);
YosukeK 6:a3829299dfd5 228 //
YosukeK 6:a3829299dfd5 229 // ret = true;
YosukeK 6:a3829299dfd5 230 // if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 231 // joint1->setAngle(servoAngle);
YosukeK 6:a3829299dfd5 232 // } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 233 // joint2->setAngle(servoAngle);
YosukeK 6:a3829299dfd5 234 // } else {
YosukeK 6:a3829299dfd5 235 // ret = false;
YosukeK 6:a3829299dfd5 236 // }
YosukeK 6:a3829299dfd5 237 //
YosukeK 6:a3829299dfd5 238 // return ret;
YosukeK 6:a3829299dfd5 239 //}
YosukeK 6:a3829299dfd5 240
YosukeK 6:a3829299dfd5 241
YosukeK 6:a3829299dfd5 242 /*
YosukeK 6:a3829299dfd5 243 *
YosukeK 6:a3829299dfd5 244 */
YosukeK 6:a3829299dfd5 245 bool initialize() {
YosukeK 6:a3829299dfd5 246 //pc.printf("init");
YosukeK 6:a3829299dfd5 247
YosukeK 6:a3829299dfd5 248 nyandaBeam = 0;
YosukeK 4:039a7d1ce3e9 249
YosukeK 6:a3829299dfd5 250 shakeHead.setAngle(0);
YosukeK 6:a3829299dfd5 251 nodHead.setAngle(0);
YosukeK 6:a3829299dfd5 252 leftArm.setAngle(0);
YosukeK 6:a3829299dfd5 253 leftShoulder.setAngle(0);
YosukeK 6:a3829299dfd5 254 rightArm.setAngle(0);
YosukeK 6:a3829299dfd5 255 rightShoulder.setAngle(0);
YosukeK 6:a3829299dfd5 256
YosukeK 6:a3829299dfd5 257 //Roomba
YosukeK 6:a3829299dfd5 258 SendData::SendData *data = new SendData(0, 0);
YosukeK 6:a3829299dfd5 259 base.set(data);
YosukeK 6:a3829299dfd5 260 delete data;
YosukeK 6:a3829299dfd5 261
YosukeK 6:a3829299dfd5 262 return true;
YosukeK 3:9ac9fdf1856e 263 }
YosukeK 0:a9b204e27472 264
YosukeK 3:9ac9fdf1856e 265
YosukeK 3:9ac9fdf1856e 266 /*
YosukeK 6:a3829299dfd5 267 *
YosukeK 6:a3829299dfd5 268 */
YosukeK 6:a3829299dfd5 269 void switchNyandaBeam(bool isFiring) {
YosukeK 6:a3829299dfd5 270 //pc.printf("beam");
YosukeK 6:a3829299dfd5 271
YosukeK 6:a3829299dfd5 272 if (isFiring) {
YosukeK 6:a3829299dfd5 273 nyandaBeam = 1;
YosukeK 6:a3829299dfd5 274 } else {
YosukeK 6:a3829299dfd5 275 nyandaBeam = 0;
YosukeK 6:a3829299dfd5 276 }
YosukeK 6:a3829299dfd5 277 }
YosukeK 6:a3829299dfd5 278
YosukeK 6:a3829299dfd5 279
YosukeK 6:a3829299dfd5 280 //給仕機を操作する
YosukeK 6:a3829299dfd5 281 void controlOyatsuFeeder(bool isMoving) {
YosukeK 6:a3829299dfd5 282 //pc.printf("feeder");
YosukeK 6:a3829299dfd5 283
YosukeK 6:a3829299dfd5 284 int feederDegree = 0;
YosukeK 6:a3829299dfd5 285
YosukeK 6:a3829299dfd5 286 if (isMoving) {
YosukeK 6:a3829299dfd5 287 if (oyatsuCnt < 3) {
YosukeK 6:a3829299dfd5 288 oyatsuCnt++;
YosukeK 6:a3829299dfd5 289 }
YosukeK 6:a3829299dfd5 290 //60, 90, 120
YosukeK 6:a3829299dfd5 291 feederDegree = 60 + 30 * (oyatsuCnt - 1);
YosukeK 6:a3829299dfd5 292 } else {
YosukeK 6:a3829299dfd5 293 oyatsuCnt = 0;
YosukeK 6:a3829299dfd5 294 }
YosukeK 6:a3829299dfd5 295
YosukeK 6:a3829299dfd5 296 oyatsuFeeder.setAngle(feederDegree);
YosukeK 6:a3829299dfd5 297 }
YosukeK 6:a3829299dfd5 298
YosukeK 6:a3829299dfd5 299
YosukeK 6:a3829299dfd5 300
YosukeK 6:a3829299dfd5 301 /*
YosukeK 3:9ac9fdf1856e 302 *
YosukeK 3:9ac9fdf1856e 303 */
YosukeK 3:9ac9fdf1856e 304 int serial_gets(char *buffer, int size) {
YosukeK 5:4c87a8401f7c 305 if (!pc.readable()) {
YosukeK 0:a9b204e27472 306 return 0;
YosukeK 5:4c87a8401f7c 307 }
YosukeK 0:a9b204e27472 308
YosukeK 0:a9b204e27472 309 int i = 0;
YosukeK 0:a9b204e27472 310 while (i < size - 1) {
YosukeK 0:a9b204e27472 311 if (pc.readable()) {
YosukeK 0:a9b204e27472 312 buffer[i] = pc.getc();
YosukeK 0:a9b204e27472 313 //pc.printf("%c", buffer[i]);
YosukeK 0:a9b204e27472 314
YosukeK 0:a9b204e27472 315 if (buffer[i] == '\r') {
YosukeK 7:3fcb0d1c41aa 316 //--i;
YosukeK 7:3fcb0d1c41aa 317 break;
YosukeK 0:a9b204e27472 318 }
YosukeK 0:a9b204e27472 319 if (buffer[i] == '\n') {
YosukeK 4:039a7d1ce3e9 320
YosukeK 0:a9b204e27472 321 break;
YosukeK 0:a9b204e27472 322 }
YosukeK 0:a9b204e27472 323 ++i;
YosukeK 0:a9b204e27472 324 }/* else {
YosukeK 0:a9b204e27472 325 wait_ms(1);
YosukeK 0:a9b204e27472 326 }*/
YosukeK 0:a9b204e27472 327 }
YosukeK 0:a9b204e27472 328 buffer[i] = '\0';
YosukeK 0:a9b204e27472 329 //pc.printf("%s", buffer);
YosukeK 0:a9b204e27472 330 return i;
YosukeK 0:a9b204e27472 331 }
YosukeK 4:039a7d1ce3e9 332
YosukeK 4:039a7d1ce3e9 333
YosukeK 4:039a7d1ce3e9 334
YosukeK 4:039a7d1ce3e9 335 ///*/*
YosukeK 4:039a7d1ce3e9 336 // *
YosukeK 4:039a7d1ce3e9 337 // */
YosukeK 4:039a7d1ce3e9 338 //int getMoveCommand(char opcode, char* before, char* converted)
YosukeK 4:039a7d1ce3e9 339 //{
YosukeK 4:039a7d1ce3e9 340 // char buff[10];
YosukeK 4:039a7d1ce3e9 341 // int temp;
YosukeK 4:039a7d1ce3e9 342 // int cont = 0;
YosukeK 4:039a7d1ce3e9 343 //
YosukeK 4:039a7d1ce3e9 344 // converted[cont] = opcode;
YosukeK 4:039a7d1ce3e9 345 // ++cont;
YosukeK 4:039a7d1ce3e9 346 // cont = cont + convertHexToAscString(before, 4, converted + cont);
YosukeK 4:039a7d1ce3e9 347 // cont = cont + convertHexToAscString(before + 4, 4, converted + cont);
YosukeK 4:039a7d1ce3e9 348 // converted[cont] = '\0';
YosukeK 4:039a7d1ce3e9 349 //
YosukeK 4:039a7d1ce3e9 350 //#ifdef DEBUG
YosukeK 4:039a7d1ce3e9 351 // int i = 0;
YosukeK 4:039a7d1ce3e9 352 // for (i = 0; i < cont; i++) {
YosukeK 4:039a7d1ce3e9 353 // printf("%x\n", *(converted + i));
YosukeK 4:039a7d1ce3e9 354 // }
YosukeK 4:039a7d1ce3e9 355 //#endif
YosukeK 4:039a7d1ce3e9 356 //
YosukeK 4:039a7d1ce3e9 357 // return strlen(converted);
YosukeK 4:039a7d1ce3e9 358 //}*/
YosukeK 4:039a7d1ce3e9 359 //
YosukeK 4:039a7d1ce3e9 360 ////TBD:Create Utility Class. And move under method add static
YosukeK 4:039a7d1ce3e9 361 //int convertDecToAsc(char* target, int length, int* converted) {
YosukeK 4:039a7d1ce3e9 362 // char buff[length + 1];
YosukeK 4:039a7d1ce3e9 363 // int temp;
YosukeK 4:039a7d1ce3e9 364 //
YosukeK 4:039a7d1ce3e9 365 // //strncpy(buff, before, length);
YosukeK 4:039a7d1ce3e9 366 // buff[length] = '\0';
YosukeK 4:039a7d1ce3e9 367 // //printf("ASC:%s\n", buff);
YosukeK 4:039a7d1ce3e9 368 //
YosukeK 4:039a7d1ce3e9 369 // temp = atoi(buff);
YosukeK 4:039a7d1ce3e9 370 // //printf("DEC:%d\n", temp);
YosukeK 4:039a7d1ce3e9 371 //
YosukeK 4:039a7d1ce3e9 372 // //converted = temp;
YosukeK 4:039a7d1ce3e9 373 //
YosukeK 4:039a7d1ce3e9 374 // return 0;//strlen(converted);
YosukeK 4:039a7d1ce3e9 375 //}
YosukeK 4:039a7d1ce3e9 376
YosukeK 4:039a7d1ce3e9 377 //
YosukeK 4:039a7d1ce3e9 378 ///*
YosukeK 4:039a7d1ce3e9 379 // *
YosukeK 4:039a7d1ce3e9 380 // */
YosukeK 4:039a7d1ce3e9 381 //int convertHexToAscString(char* before, int length, char* converted)
YosukeK 4:039a7d1ce3e9 382 //{
YosukeK 4:039a7d1ce3e9 383 // char buff[length + 1];
YosukeK 4:039a7d1ce3e9 384 // int temp;
YosukeK 4:039a7d1ce3e9 385 //
YosukeK 4:039a7d1ce3e9 386 // printf("Param:%s\n", before);
YosukeK 4:039a7d1ce3e9 387 //
YosukeK 4:039a7d1ce3e9 388 // strncpy(buff, before, length);
YosukeK 4:039a7d1ce3e9 389 // buff[length] = '\0';
YosukeK 4:039a7d1ce3e9 390 // printf("ASC:%s\n", buff);
YosukeK 4:039a7d1ce3e9 391 //
YosukeK 4:039a7d1ce3e9 392 // temp = atoi(buff);
YosukeK 4:039a7d1ce3e9 393 // printf("DEC:%d\n", temp);
YosukeK 4:039a7d1ce3e9 394 //
YosukeK 4:039a7d1ce3e9 395 // converted[0] = ((temp & 0xFF00) >> 8);
YosukeK 4:039a7d1ce3e9 396 // converted[1] = (temp & 0xFF);
YosukeK 4:039a7d1ce3e9 397 // converted[2] = '\0';
YosukeK 4:039a7d1ce3e9 398 // printf("HEX(MSB):%x\n", converted[0]);
YosukeK 4:039a7d1ce3e9 399 // printf("HEX(LSB):%x\n", converted[1]);
YosukeK 4:039a7d1ce3e9 400 //
YosukeK 4:039a7d1ce3e9 401 // return strlen(converted);
YosukeK 4:039a7d1ce3e9 402 //}
YosukeK 4:039a7d1ce3e9 403 //
YosukeK 6:a3829299dfd5 404
YosukeK 6:a3829299dfd5 405
YosukeK 6:a3829299dfd5 406
YosukeK 6:a3829299dfd5 407
YosukeK 6:a3829299dfd5 408
YosukeK 6:a3829299dfd5 409
YosukeK 6:a3829299dfd5 410
YosukeK 6:a3829299dfd5 411 // switch (buffer[2]) {
YosukeK 6:a3829299dfd5 412 // case 'Z':
YosukeK 6:a3829299dfd5 413 // ret = initialize();
YosukeK 6:a3829299dfd5 414 // break;
YosukeK 6:a3829299dfd5 415 //
YosukeK 6:a3829299dfd5 416 // case 'N':
YosukeK 6:a3829299dfd5 417 // bool isFiring = ( buffer[3] == '1' ) ? true : false;
YosukeK 6:a3829299dfd5 418 // switchNyandaBeam(isFiring);
YosukeK 6:a3829299dfd5 419 // ret = isFiring;
YosukeK 6:a3829299dfd5 420 // break;
YosukeK 6:a3829299dfd5 421 //
YosukeK 6:a3829299dfd5 422 // case 'M'://CTM+500-500
YosukeK 6:a3829299dfd5 423 // ret = true;
YosukeK 6:a3829299dfd5 424 //
YosukeK 6:a3829299dfd5 425 // char buff[4+1];
YosukeK 6:a3829299dfd5 426 // int temp;
YosukeK 6:a3829299dfd5 427 //
YosukeK 6:a3829299dfd5 428 // //pc.printf("Param:%s\n", before);
YosukeK 6:a3829299dfd5 429 // strncpy(buff, buffer + 3, 4);
YosukeK 6:a3829299dfd5 430 // buff[4] = '\0';
YosukeK 6:a3829299dfd5 431 // //pc.printf("ASC:%s\n", buff);
YosukeK 6:a3829299dfd5 432 // temp = atoi(buff);
YosukeK 6:a3829299dfd5 433 // e.leftWheetVelocity = temp;
YosukeK 6:a3829299dfd5 434 // //printf("Param:%s\n", before);
YosukeK 6:a3829299dfd5 435 // strncpy(buff, buffer + 7, 4);//(char*)buffer[3+4]
YosukeK 6:a3829299dfd5 436 // buff[4] = '\0';
YosukeK 6:a3829299dfd5 437 // //pc. printf("ASC:%s\n", buff);
YosukeK 6:a3829299dfd5 438 // temp = atoi(buff);
YosukeK 6:a3829299dfd5 439 // e.rightWheelVelocity = temp;
YosukeK 6:a3829299dfd5 440 //
YosukeK 6:a3829299dfd5 441 // break;
YosukeK 6:a3829299dfd5 442 //
YosukeK 6:a3829299dfd5 443 // case 'O':
YosukeK 6:a3829299dfd5 444 // ret = true;
YosukeK 6:a3829299dfd5 445 //
YosukeK 6:a3829299dfd5 446 // if (buffer[3] == '0') {
YosukeK 6:a3829299dfd5 447 // oyatsuCnt = 0;
YosukeK 6:a3829299dfd5 448 // } else if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 449 // if (oyatsuCnt < 3) {
YosukeK 6:a3829299dfd5 450 // oyatsuCnt++;
YosukeK 6:a3829299dfd5 451 // }
YosukeK 6:a3829299dfd5 452 //
YosukeK 6:a3829299dfd5 453 // switch (oyatsuCnt) {
YosukeK 6:a3829299dfd5 454 // case 0:
YosukeK 6:a3829299dfd5 455 // oyatsuFeeder.setAngle(0);
YosukeK 6:a3829299dfd5 456 // break;
YosukeK 6:a3829299dfd5 457 //
YosukeK 6:a3829299dfd5 458 // case 1:
YosukeK 6:a3829299dfd5 459 // oyatsuFeeder.setAngle(60);
YosukeK 6:a3829299dfd5 460 // break;
YosukeK 6:a3829299dfd5 461 //
YosukeK 6:a3829299dfd5 462 // case 2:
YosukeK 6:a3829299dfd5 463 // oyatsuFeeder.setAngle(90);
YosukeK 6:a3829299dfd5 464 // break;
YosukeK 6:a3829299dfd5 465 //
YosukeK 6:a3829299dfd5 466 // case 3:
YosukeK 6:a3829299dfd5 467 // oyatsuFeeder.setAngle(120);
YosukeK 6:a3829299dfd5 468 // break;
YosukeK 6:a3829299dfd5 469 //
YosukeK 6:a3829299dfd5 470 // default:
YosukeK 6:a3829299dfd5 471 // //oyatsuFeeder.setAngle(0);
YosukeK 6:a3829299dfd5 472 // break;
YosukeK 6:a3829299dfd5 473 // }
YosukeK 6:a3829299dfd5 474 // } else {
YosukeK 6:a3829299dfd5 475 // ret = false;
YosukeK 6:a3829299dfd5 476 // }
YosukeK 6:a3829299dfd5 477 //
YosukeK 6:a3829299dfd5 478 // break;
YosukeK 6:a3829299dfd5 479 //
YosukeK 6:a3829299dfd5 480 // case 'H':
YosukeK 6:a3829299dfd5 481 // int servoAngle = -1;
YosukeK 6:a3829299dfd5 482 // servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 483 //
YosukeK 6:a3829299dfd5 484 // ret = true;
YosukeK 6:a3829299dfd5 485 // if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 486 // shakeHead.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 487 // } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 488 // nodHead.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 489 // } else {
YosukeK 6:a3829299dfd5 490 // ret = false;
YosukeK 6:a3829299dfd5 491 // }
YosukeK 6:a3829299dfd5 492 // break;
YosukeK 6:a3829299dfd5 493 //
YosukeK 6:a3829299dfd5 494 // case 'L':
YosukeK 6:a3829299dfd5 495 // servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 496 //
YosukeK 6:a3829299dfd5 497 // ret = true;
YosukeK 6:a3829299dfd5 498 // if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 499 // leftShoulder.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 500 // } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 501 // leftArm.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 502 // } else {
YosukeK 6:a3829299dfd5 503 // ret = false;
YosukeK 6:a3829299dfd5 504 // }
YosukeK 6:a3829299dfd5 505 // break;
YosukeK 6:a3829299dfd5 506 //
YosukeK 6:a3829299dfd5 507 // case 'R':
YosukeK 6:a3829299dfd5 508 // servoAngle = atoi(buffer + 4);
YosukeK 6:a3829299dfd5 509 //
YosukeK 6:a3829299dfd5 510 // ret = true;
YosukeK 6:a3829299dfd5 511 // if (buffer[3] == '1') {
YosukeK 6:a3829299dfd5 512 // rightShoulder.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 513 // } else if (buffer[3] == '2') {
YosukeK 6:a3829299dfd5 514 // rightArm.setAngle(servoAngle);
YosukeK 6:a3829299dfd5 515 // } else {
YosukeK 6:a3829299dfd5 516 // ret = false;
YosukeK 6:a3829299dfd5 517 // }
YosukeK 6:a3829299dfd5 518 // break;
YosukeK 6:a3829299dfd5 519 //
YosukeK 6:a3829299dfd5 520 // default:
YosukeK 6:a3829299dfd5 521 // ret = false;
YosukeK 6:a3829299dfd5 522 // break;
YosukeK 6:a3829299dfd5 523 // }
YosukeK 6:a3829299dfd5 524 // }