Diff: main.cpp

Revision:

1:5f6dd444850a

Parent:

0:a9b204e27472

Child:

2:91135f19ac12

--- a/main.cpp	Thu Sep 11 13:45:07 2014 +0000
+++ b/main.cpp	Sat Sep 20 07:04:09 2014 +0000
@@ -1,5 +1,6 @@
 #include <mbed.h>
 #include <PwmServo.h>
+#include <ExtendedServo.h>
 #include <RobotArm.h>
 #include <def_resources.h>
 #include <Serial.h>
@@ -35,12 +36,13 @@
 int main() {
   myled = 1;
   
-  head0 = 90;
-  head0 = head0 - 10;
-  head1 = 90;
-  head1 = head1 - 20;
-  arm1.actuate(head0, head1);
-  
+  shakeHead.setAngle(0);
+  nodHead.setAngle(0);
+  leftArm.setAngle(0);
+  leftShoulder.setAngle(0);
+  rightArm.setAngle(0);
+  rightShoulder.setAngle(0);
+
   while(1) {
     char buffer[BUFFER_SIZE];
     
@@ -51,54 +53,72 @@
       if (strncmp(buffer, "CT", 2) == 0) {
         
         myled = !myled;
+
+        bool ret = false;
         
         if (size == 2) {
-          pc.printf("\r\nOK\r\n");
+          ret = true;
         } else if (size >= 3) {
           switch (buffer[2]) {
           case 'O':
-          
+           
+            //TODO:Move OYATSU bender.
+            
+            ret = true;
             break;
           case 'H':
           
             int servoAngle = -1;
             servoAngle = atoi((buffer+4));
-            //printf("%c\n", buffer[3]);
-            //printf("%d\n", servoAngle);
+            
+            ret = true;
+            if (buffer[3] == '1') {
+              shakeHead.setAngle(servoAngle);
+            } else if (buffer[3] == '2') {
+              nodHead.setAngle(servoAngle);
+            } else {
+              ret = false;
+            }
+            break;
             
-            servoAngle += 90;
-            servoAngle = 180 - servoAngle;
-                        
-            if (servoAngle < 20) {
-              servoAngle = 20;
+          case 'L':
+            servoAngle = atoi((buffer+4));
+            
+            ret = true;
+            if (buffer[3] == '1') {
+              leftArm.setAngle(servoAngle);
+            } else if (buffer[3] == '2') {
+              leftShoulder.setAngle(servoAngle);
+            } else {
+              ret = false;
             }
-            if (servoAngle > 160) {
-              servoAngle = 160;
-            }
-                        
+            break;
+          
+          case 'R':
+            servoAngle = atoi((buffer+4));
 
+            ret = true;
             if (buffer[3] == '1') {
-              head0 = servoAngle;
-              head0 = head0 - 10;
-              //printf("%d\n", head0);
+              rightArm.setAngle(servoAngle);
             } else if (buffer[3] == '2') {
-              head1 = servoAngle;
-              head1 = head1 - 20;
-              //printf("%d\n", head1);
+              rightShoulder.setAngle(servoAngle);
             } else {
-                
+              ret = false;   
             }
-
-            arm1.actuate(head0, head1);
-
+            break;
+            
+          default:
+            ret = false;
             break;
           }
-          
-          pc.printf("\r\nOK\r\n");
         }
         
+        if (ret) {
+          pc.printf("\r\nOK\r\n");
+        } else {
+          pc.printf("\r\nNG\r\n");  
+        }
       }
-      
     }
     
   }
@@ -206,7 +226,7 @@
             break;
           
           case 'H':
-            //myled = 1;            
+            //myled = 1;         
             int servoAngle = -1;
             servoAngle = atoi((receive+4));