Diff: main.cpp

Revision:

3:9ac9fdf1856e

Parent:

2:91135f19ac12

Child:

4:039a7d1ce3e9

--- a/main.cpp	Tue Sep 23 04:45:25 2014 +0000
+++ b/main.cpp	Sat Nov 08 02:45:59 2014 +0000
@@ -1,7 +1,8 @@
 #include <mbed.h>
 #include <PwmServo.h>
 #include <ExtendedServo.h>
-#include <RobotArm.h>
+#include <DelayServo.h>
+//#include <RobotArm.h>
 #include <def_resources.h>
 #include <Serial.h>
 
@@ -16,40 +17,31 @@
 
 #define DEBUG
 #define BUFFER_SIZE 11
-#define COMMAND_TOTAL 3
-
+//#define COMMAND_TOTAL 3
 
 
 char receive[BUFFER_SIZE]; 
 //char* p_receive;
-int recvPnt = 0;
-
-int head0;
-int head1;
+//int recvPnt = 0;
 
 int oyatsuCnt = 0;
 
-void pc_rx ();
+int serial_gets(char *buffer, int size);
 
-int serial_gets (char *buffer, int size);
+int initialize();
 
 int main() {
-  myled = 1;
-  
-  shakeHead.setAngle(0);
-  nodHead.setAngle(0);
-  leftArm.setAngle(0);
-  leftShoulder.setAngle(0);
-  rightArm.setAngle(0);
-  rightShoulder.setAngle(0);
+
+  initialize();
 
   while(1) {
     char buffer[BUFFER_SIZE];
-    
+    // コマンド受信
     int size = serial_gets(buffer, BUFFER_SIZE);
     if (size > 0) {
-      pc.printf("%s", buffer);
-      
+      //pc.printf("%s", buffer);
+
+      // コマンド判定
       if (strncmp(buffer, "CT", 2) == 0) {
         
         myled = !myled;
@@ -59,8 +51,19 @@
         if (size == 2) {
           ret = true;
         } else if (size >= 3) {
+          
+          
+          
           switch (buffer[2]) {
+          case 'Z':
+          
+            initialize();
+          
+            ret = true;
+            break;
+            
           case 'N':
+          
             ret = true;
             if (buffer[3] == '0') {
               nyandaLight = 0;
@@ -127,9 +130,9 @@
             
             ret = true;
             if (buffer[3] == '1') {
-              leftArm.setAngle(servoAngle);
+              leftShoulder.setAngle(servoAngle);
             } else if (buffer[3] == '2') {
-              leftShoulder.setAngle(servoAngle);
+              leftArm.setAngle(servoAngle);
             } else {
               ret = false;
             }
@@ -140,9 +143,9 @@
 
             ret = true;
             if (buffer[3] == '1') {
-              rightArm.setAngle(servoAngle);
+              rightShoulder.setAngle(servoAngle);
             } else if (buffer[3] == '2') {
-              rightShoulder.setAngle(servoAngle);
+              rightArm.setAngle(servoAngle);
             } else {
               ret = false;   
             }
@@ -166,8 +169,24 @@
 }
 
 
+int initialize() {
+  myled = 1;
+  
+  nyandaLight = 0;
+  
+  shakeHead.setAngle(0);
+  nodHead.setAngle(0);
+  leftArm.setAngle(0);
+  leftShoulder.setAngle(0);
+  rightArm.setAngle(0);
+  rightShoulder.setAngle(0);
+}
 
-int serial_gets (char *buffer, int size) {
+
+/*
+ * 
+ */
+int serial_gets(char *buffer, int size) {
   /*if (!pc.readable()) {
     return 0;
   }*/
@@ -193,169 +212,3 @@
   //pc.printf("%s", buffer);
   return i;
 }
-
-
-//int main()
-//{
-//  head0 = 90;
-//  head0 = head0 - 10;
-//  head1 = 90;
-//  head1 = head1 - 20;
-
-  //myled = 0;
-  
-  //clear buffer
-//  memset(receive, 0, BUFFER_SIZE);
-  //receive[BUFFER_SIZE-1] = '\0';
-  //p_receive = receive;
-
-  //TODO:Create arm instance.
-  
-  
-//  arm1.actuate(head0, head1);
-  
-  
-  //pc.attach(pc_rx, Serial::RxIrq);
-  
-//  while(1) {
-//  }    
-
-//    int i;
-//
-//    while(1) {
-//        for(i=0; i<180; i++) {
-//            arm1.actuate(i,i);
-//            wait(0.005);
-//        }
-//        for(i=180; i>0; i--) {
-//            arm1.actuate(i,i);
-//            wait(0.005);
-//        }
-//    }
-//}
-
-void pc_rx () {
-  int result = -1;
-  
-  char c = pc.getc();
-  //pc.printf("%02X", (unsigned char)c);
-  pc.printf("%c", (unsigned char)c);
-  
-  if (c == '\r') {
-    //Nothign to do.
-  } else if (c == '\n') {
-    receive[recvPnt] = '\0';
-    ++recvPnt;    
-  } else {
-    receive[recvPnt] = c;
-    ++recvPnt;
-  }
-
-  
-  if (c == '\n') {
-    if (receive[0] == 'C' && receive[1] == 'T') {
-      
-      myled = 1 - myled;
-      
-      if (recvPnt == 3) {
-        pc.printf("\r\nOK\r\n");
-        
-      } else if (recvPnt >= 4) {
-        switch (receive[2]) {
-          case 'O':
-          
-            break;
-          
-          case 'H':
-            //myled = 1;         
-            int servoAngle = -1;
-            servoAngle = atoi((receive+4));
-            
-            servoAngle += 90;
-            servoAngle = 180 - servoAngle;
-                        
-            if (servoAngle < 20) {
-              servoAngle = 20;
-            }
-            if (servoAngle > 160) {
-              servoAngle = 160;
-            }
-                        
-            //printf("%d\n", servoAngle);
-
-            int servoNo = -1;
-            
-            if (receive[3] == '1') {
-              servoNo = 0;
-              head0 = servoAngle;
-              head0 = head0 - 10;
-            } else if (receive[3] == '2') {
-              servoNo = 1;
-              head1 = servoAngle;
-              head1 = head1 - 20;
-            } else {
-              servoNo = -1;
-            }
-            //printf("%d\n", servoNo);
-
-            //arm1.actuate(servoNo, servoAngle);
-            arm1.actuate(head0, head1);
-
-            result = 0;
-            pc.printf("\r\nOK\r\n");
-            
-            break;
-          
-          case 'L':
-            pc.printf("\r\nOK\r\n");
-            break;
-
-          case 'R':
-            pc.printf("\r\nOK\r\n");
-            break;
-          
-          case 'N':
-          
-            if (receive[3] == '1') {
-              myled = 1;
-            } else {
-              myled = 0;
-            }
-          
-            pc.printf("\r\nOK\r\n");
-            break;
-          
-          case 'M':
-            pc.printf("\r\nOK\r\n");
-            break;
-
-          case 'Z':
-            pc.printf("\r\nOK\r\n");
-            break;
-          
-          default:
-          
-            result = 1;
-            pc.printf("\r\nERROR\r\n");
-            break;
-        }
-      } else {
-        result = 1;
-        pc.printf("\r\nERROR\r\n");
-      }
-    }
-    
-    //TODO:Send result the following code: CT[number]\r\n
-    
-    memset(receive, 0, BUFFER_SIZE);
-    recvPnt = 0;
-  }
-  
-  if (recvPnt >= BUFFER_SIZE) {
-    //myled = 0;
-    //pc.printf("NG1:Reset receive buffer.\n");
-    memset(receive, 0, BUFFER_SIZE);
-    recvPnt = 0;
-  }
-  
-}