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@3:9ac9fdf1856e, 2014-11-08 (annotated)

Committer:
YosukeK
Date:
Sat Nov 08 02:45:59 2014 +0000
Revision:
3:9ac9fdf1856e
Parent:
2:91135f19ac12
Child:
4:039a7d1ce3e9
Added DelayServo class.

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 3:9ac9fdf1856e 4 #include <DelayServo.h>
YosukeK 3:9ac9fdf1856e 5 //#include <RobotArm.h>
YosukeK 0:a9b204e27472 6 #include <def_resources.h>
YosukeK 0:a9b204e27472 7 #include <Serial.h>
YosukeK 0:a9b204e27472 8
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 0:a9b204e27472 19 #define BUFFER_SIZE 11
YosukeK 3:9ac9fdf1856e 20 //#define COMMAND_TOTAL 3
YosukeK 2:91135f19ac12 21
YosukeK 0:a9b204e27472 22
YosukeK 0:a9b204e27472 23 char receive[BUFFER_SIZE];
YosukeK 0:a9b204e27472 24 //char* p_receive;
YosukeK 3:9ac9fdf1856e 25 //int recvPnt = 0;
YosukeK 0:a9b204e27472 26
YosukeK 2:91135f19ac12 27 int oyatsuCnt = 0;
YosukeK 2:91135f19ac12 28
YosukeK 3:9ac9fdf1856e 29 int serial_gets(char *buffer, int size);
YosukeK 0:a9b204e27472 30
YosukeK 3:9ac9fdf1856e 31 int initialize();
YosukeK 0:a9b204e27472 32
YosukeK 0:a9b204e27472 33 int main() {
YosukeK 3:9ac9fdf1856e 34
YosukeK 3:9ac9fdf1856e 35 initialize();
YosukeK 1:5f6dd444850a 36
YosukeK 0:a9b204e27472 37 while(1) {
YosukeK 0:a9b204e27472 38 char buffer[BUFFER_SIZE];
YosukeK 3:9ac9fdf1856e 39 // コマンド受信
YosukeK 0:a9b204e27472 40 int size = serial_gets(buffer, BUFFER_SIZE);
YosukeK 0:a9b204e27472 41 if (size > 0) {
YosukeK 3:9ac9fdf1856e 42 //pc.printf("%s", buffer);
YosukeK 3:9ac9fdf1856e 43
YosukeK 3:9ac9fdf1856e 44 // コマンド判定
YosukeK 0:a9b204e27472 45 if (strncmp(buffer, "CT", 2) == 0) {
YosukeK 0:a9b204e27472 46
YosukeK 0:a9b204e27472 47 myled = !myled;
YosukeK 1:5f6dd444850a 48
YosukeK 1:5f6dd444850a 49 bool ret = false;
YosukeK 0:a9b204e27472 50
YosukeK 0:a9b204e27472 51 if (size == 2) {
YosukeK 1:5f6dd444850a 52 ret = true;
YosukeK 0:a9b204e27472 53 } else if (size >= 3) {
YosukeK 3:9ac9fdf1856e 54
YosukeK 3:9ac9fdf1856e 55
YosukeK 3:9ac9fdf1856e 56
YosukeK 0:a9b204e27472 57 switch (buffer[2]) {
YosukeK 3:9ac9fdf1856e 58 case 'Z':
YosukeK 3:9ac9fdf1856e 59
YosukeK 3:9ac9fdf1856e 60 initialize();
YosukeK 3:9ac9fdf1856e 61
YosukeK 3:9ac9fdf1856e 62 ret = true;
YosukeK 3:9ac9fdf1856e 63 break;
YosukeK 3:9ac9fdf1856e 64
YosukeK 2:91135f19ac12 65 case 'N':
YosukeK 3:9ac9fdf1856e 66
YosukeK 2:91135f19ac12 67 ret = true;
YosukeK 2:91135f19ac12 68 if (buffer[3] == '0') {
YosukeK 2:91135f19ac12 69 nyandaLight = 0;
YosukeK 2:91135f19ac12 70 } else if (buffer[3] == '1') {
YosukeK 2:91135f19ac12 71 nyandaLight = 1;
YosukeK 2:91135f19ac12 72 } else {
YosukeK 2:91135f19ac12 73 ret = false;
YosukeK 2:91135f19ac12 74 }
YosukeK 2:91135f19ac12 75
YosukeK 2:91135f19ac12 76 break;
YosukeK 2:91135f19ac12 77
YosukeK 0:a9b204e27472 78 case 'O':
YosukeK 2:91135f19ac12 79
YosukeK 1:5f6dd444850a 80 ret = true;
YosukeK 2:91135f19ac12 81 if (buffer[3] == '0') {
YosukeK 2:91135f19ac12 82 oyatsuCnt = 0;
YosukeK 2:91135f19ac12 83 oyatsuFeeder.setAngle(0);
YosukeK 2:91135f19ac12 84 } else if (buffer[3] == '1') {
YosukeK 2:91135f19ac12 85 if (oyatsuCnt <= 3) {
YosukeK 2:91135f19ac12 86 oyatsuCnt++;
YosukeK 2:91135f19ac12 87 }
YosukeK 2:91135f19ac12 88
YosukeK 2:91135f19ac12 89 switch (oyatsuCnt) {
YosukeK 2:91135f19ac12 90 //case 0:
YosukeK 2:91135f19ac12 91 // oyatusFeeder.setAngle(0);
YosukeK 2:91135f19ac12 92 //break;
YosukeK 2:91135f19ac12 93 case 1:
YosukeK 2:91135f19ac12 94 oyatsuFeeder.setAngle(60);
YosukeK 2:91135f19ac12 95 break;
YosukeK 2:91135f19ac12 96
YosukeK 2:91135f19ac12 97 case 2:
YosukeK 2:91135f19ac12 98 oyatsuFeeder.setAngle(90);
YosukeK 2:91135f19ac12 99 break;
YosukeK 2:91135f19ac12 100
YosukeK 2:91135f19ac12 101 case 3:
YosukeK 2:91135f19ac12 102 oyatsuFeeder.setAngle(120);
YosukeK 2:91135f19ac12 103 break;
YosukeK 2:91135f19ac12 104
YosukeK 2:91135f19ac12 105 default:
YosukeK 2:91135f19ac12 106 break;
YosukeK 2:91135f19ac12 107 }
YosukeK 2:91135f19ac12 108 } else {
YosukeK 2:91135f19ac12 109 ret = false;
YosukeK 2:91135f19ac12 110 }
YosukeK 2:91135f19ac12 111
YosukeK 0:a9b204e27472 112 break;
YosukeK 0:a9b204e27472 113 case 'H':
YosukeK 0:a9b204e27472 114
YosukeK 0:a9b204e27472 115 int servoAngle = -1;
YosukeK 0:a9b204e27472 116 servoAngle = atoi((buffer+4));
YosukeK 1:5f6dd444850a 117
YosukeK 1:5f6dd444850a 118 ret = true;
YosukeK 1:5f6dd444850a 119 if (buffer[3] == '1') {
YosukeK 1:5f6dd444850a 120 shakeHead.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 121 } else if (buffer[3] == '2') {
YosukeK 1:5f6dd444850a 122 nodHead.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 123 } else {
YosukeK 1:5f6dd444850a 124 ret = false;
YosukeK 1:5f6dd444850a 125 }
YosukeK 1:5f6dd444850a 126 break;
YosukeK 0:a9b204e27472 127
YosukeK 1:5f6dd444850a 128 case 'L':
YosukeK 1:5f6dd444850a 129 servoAngle = atoi((buffer+4));
YosukeK 1:5f6dd444850a 130
YosukeK 1:5f6dd444850a 131 ret = true;
YosukeK 1:5f6dd444850a 132 if (buffer[3] == '1') {
YosukeK 3:9ac9fdf1856e 133 leftShoulder.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 134 } else if (buffer[3] == '2') {
YosukeK 3:9ac9fdf1856e 135 leftArm.setAngle(servoAngle);
YosukeK 1:5f6dd444850a 136 } else {
YosukeK 1:5f6dd444850a 137 ret = false;
YosukeK 0:a9b204e27472 138 }
YosukeK 1:5f6dd444850a 139 break;
YosukeK 1:5f6dd444850a 140
YosukeK 1:5f6dd444850a 141 case 'R':
YosukeK 1:5f6dd444850a 142 servoAngle = atoi((buffer+4));
YosukeK 0:a9b204e27472 143
YosukeK 1:5f6dd444850a 144 ret = true;
YosukeK 0:a9b204e27472 145 if (buffer[3] == '1') {
YosukeK 3:9ac9fdf1856e 146 rightShoulder.setAngle(servoAngle);
YosukeK 0:a9b204e27472 147 } else if (buffer[3] == '2') {
YosukeK 3:9ac9fdf1856e 148 rightArm.setAngle(servoAngle);
YosukeK 0:a9b204e27472 149 } else {
YosukeK 1:5f6dd444850a 150 ret = false;
YosukeK 0:a9b204e27472 151 }
YosukeK 1:5f6dd444850a 152 break;
YosukeK 1:5f6dd444850a 153
YosukeK 1:5f6dd444850a 154 default:
YosukeK 1:5f6dd444850a 155 ret = false;
YosukeK 0:a9b204e27472 156 break;
YosukeK 0:a9b204e27472 157 }
YosukeK 0:a9b204e27472 158 }
YosukeK 0:a9b204e27472 159
YosukeK 1:5f6dd444850a 160 if (ret) {
YosukeK 1:5f6dd444850a 161 pc.printf("\r\nOK\r\n");
YosukeK 1:5f6dd444850a 162 } else {
YosukeK 1:5f6dd444850a 163 pc.printf("\r\nNG\r\n");
YosukeK 1:5f6dd444850a 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
YosukeK 0:a9b204e27472 171
YosukeK 3:9ac9fdf1856e 172 int initialize() {
YosukeK 3:9ac9fdf1856e 173 myled = 1;
YosukeK 3:9ac9fdf1856e 174
YosukeK 3:9ac9fdf1856e 175 nyandaLight = 0;
YosukeK 3:9ac9fdf1856e 176
YosukeK 3:9ac9fdf1856e 177 shakeHead.setAngle(0);
YosukeK 3:9ac9fdf1856e 178 nodHead.setAngle(0);
YosukeK 3:9ac9fdf1856e 179 leftArm.setAngle(0);
YosukeK 3:9ac9fdf1856e 180 leftShoulder.setAngle(0);
YosukeK 3:9ac9fdf1856e 181 rightArm.setAngle(0);
YosukeK 3:9ac9fdf1856e 182 rightShoulder.setAngle(0);
YosukeK 3:9ac9fdf1856e 183 }
YosukeK 0:a9b204e27472 184
YosukeK 3:9ac9fdf1856e 185
YosukeK 3:9ac9fdf1856e 186 /*
YosukeK 3:9ac9fdf1856e 187 *
YosukeK 3:9ac9fdf1856e 188 */
YosukeK 3:9ac9fdf1856e 189 int serial_gets(char *buffer, int size) {
YosukeK 0:a9b204e27472 190 /*if (!pc.readable()) {
YosukeK 0:a9b204e27472 191 return 0;
YosukeK 0:a9b204e27472 192 }*/
YosukeK 0:a9b204e27472 193
YosukeK 0:a9b204e27472 194 int i = 0;
YosukeK 0:a9b204e27472 195 while (i < size - 1) {
YosukeK 0:a9b204e27472 196 if (pc.readable()) {
YosukeK 0:a9b204e27472 197 buffer[i] = pc.getc();
YosukeK 0:a9b204e27472 198 //pc.printf("%c", buffer[i]);
YosukeK 0:a9b204e27472 199
YosukeK 0:a9b204e27472 200 if (buffer[i] == '\r') {
YosukeK 0:a9b204e27472 201 --i;
YosukeK 0:a9b204e27472 202 }
YosukeK 0:a9b204e27472 203 if (buffer[i] == '\n') {
YosukeK 0:a9b204e27472 204 break;
YosukeK 0:a9b204e27472 205 }
YosukeK 0:a9b204e27472 206 ++i;
YosukeK 0:a9b204e27472 207 }/* else {
YosukeK 0:a9b204e27472 208 wait_ms(1);
YosukeK 0:a9b204e27472 209 }*/
YosukeK 0:a9b204e27472 210 }
YosukeK 0:a9b204e27472 211 buffer[i] = '\0';
YosukeK 0:a9b204e27472 212 //pc.printf("%s", buffer);
YosukeK 0:a9b204e27472 213 return i;
YosukeK 0:a9b204e27472 214 }