Diff: main.cpp

Revision:

8:bbea423c9206

Parent:

7:58eb1e1e9143

Child:

9:6f3963858d89

--- a/main.cpp	Mon Apr 27 04:21:44 2015 +0000
+++ b/main.cpp	Mon Apr 27 11:04:34 2015 +0000
@@ -8,6 +8,18 @@
 Serial pc(USBTX, USBRX); // tx, rx
 DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4);
 
+struct ArmPacketBits {
+                      unsigned int mode : 2;
+                      unsigned int linear : 2;
+                      unsigned int pitch : 2;
+                      unsigned int yaw : 2;
+                     };
+
+union ArmPacket {
+                 struct ArmPacketBits b;
+                 unsigned char as_byte;
+                };
+
 /*--Dynamixel:begin-----------------------------------------------------------------------------------------*/
 #include "AX12.h"
 #include "MX28.h"
@@ -59,29 +71,44 @@
     if(DYNA_DEBUG) printf("\nDyna_SetGoal\n");
     switch(linear_mode){
         case 0: break;
-        case 1: linear.SetTorqueLimit(1); linear_goal-=2; break;
-        case 2: linear.SetTorqueLimit(1); linear_goal+=2; break;
+        case 1: 
+            linear.SetTorqueLimit(1); linear_goal-=10;  
+            if(linear_goal<linear_min) linear_goal=linear_min;
+            break;
+        case 2: 
+            linear.SetTorqueLimit(1); linear_goal+=20;
+            if(linear_goal>linear_MAX) linear_goal=linear_MAX;
+            break;
     }
-    if(linear_goal>linear_MAX) linear_goal=linear_MAX; 
-    if(linear_goal<linear_min) linear_goal=linear_min;
+    
     linear.SetGoal(linear_goal);
     
     switch(pitch_mode){
         case 0: break;
-        case 1: pitch_goal-=3; break;
-        case 2: pitch_goal+=3; break;
+        case 1:
+            pitch_goal-=10;
+            if(pitch_goal<pitch_min) pitch_goal=pitch_min;
+            break;
+        case 2:
+            pitch_goal+=10;
+            if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; 
+            break;
     }    
-    if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; 
-    if(pitch_goal<pitch_min) pitch_goal=pitch_min;
+    
     pitch.SetGoal(pitch_goal);
     
     switch(yaw_mode){
         case 0: break;
-        case 1: yaw_goal-=3; break;
-        case 2: yaw_goal+=3; break;
+        case 1:
+            yaw_goal-=10;
+            if(pitch_goal<-2520) pitch_goal=-2520;
+            break;
+        case 2:
+            yaw_goal+=10;
+            if(pitch_goal>2520) pitch_goal=2520; 
+            break;
     }
-    if(pitch_goal>2520) pitch_goal=2520; 
-    if(pitch_goal<-2520) pitch_goal=-2520;
+
     yaw.SetGoal(yaw_goal);
 }
 
@@ -257,32 +284,39 @@
 
 
 /*---------------
-    MAIN ROOP
+    MAIN LOOP
 ----------------*/
 #define DEBUG 0
 
 int main() {
-
+    myled1=1; myled2=1; myled3=1; myled4=1;
     pc.baud(38400);
     
     char Dyna_data[50];
     char Thermo_data[200];
     float CO2_data;
     
+    myled1=1; myled2=1; myled3=0; myled4=0;
     Dyna_init();
+    Dyna_home_position();
+    wait(2);
+    Dyna_home_position();
+    wait(2);
+    myled1=0; myled2=0; myled3=1; myled4=0;
     
-    unsigned char a = pc.getc();   
-    if(a==7) printf("arm\n");
-    
+    union ArmPacket Read_data;
+        
     char Send_data[250];
-    int Read_data;
+    //int Read_data;
     int Joy_mode, linear_mode, pitch_mode, yaw_mode;
     
     while(1) {
-        
-        Read_data=pc.getc();
+        //Read_data=pc.getc();
+        Read_data.as_byte=pc.getc();
         wait_ms(1);
-        Joy_mode = Read_data & 0x03;; //ジョイスティックのモード判定
+        if(Read_data.as_byte == 7) {printf("arm\n"); continue;}
+        //Joy_mode = Read_data & 0x03; //ジョイスティックのモード判定
+        Joy_mode = Read_data.b.mode; //ジョイスティックのモード判定
         if(DEBUG) printf("Joy_mode : %d\n",Joy_mode);
         
         switch(Joy_mode){
@@ -290,21 +324,33 @@
                 myled1=0; myled2=0; myled3=0; myled4=0;
             /*--Dynamixel:begin---------------------------------------------------------------*/       
                 //Dynamixelへの命令を判定
-                linear_mode = Read_data & 0xc0; linear_mode = linear_mode >> 6; if(DEBUG) printf("linear_mode : %d\n",linear_mode);
-                pitch_mode = Read_data & 0x30; pitch_mode = pitch_mode >> 4; if(DEBUG) printf("pitch_mode : %d\n",pitch_mode);
-                yaw_mode = Read_data & 0x0c; yaw_mode = yaw_mode >> 2; if(DEBUG) printf("yaw_mode : %d\n",yaw_mode);
-
-                //現在の角度・電圧・電流を取得
-                myled1=1;
-                Dyna_GetData(Dyna_data);
-                myled1=0;
+                //linear_mode = Read_data & 0xc0; linear_mode = linear_mode >> 6; if(DEBUG) printf("linear_mode : %d\n",linear_mode);
+//                pitch_mode = Read_data & 0x30; pitch_mode = pitch_mode >> 4; if(DEBUG) printf("pitch_mode : %d\n",pitch_mode);
+//                yaw_mode = Read_data & 0x0c; yaw_mode = yaw_mode >> 2; if(DEBUG) printf("yaw_mode : %d\n",yaw_mode);
+                linear_mode = Read_data.b.linear;
+                pitch_mode = Read_data.b.pitch;
+                yaw_mode = Read_data.b.yaw;
                 
                 //目標角度を変更
                 myled2=1;
                 Dyna_SetGoal(linear_mode, pitch_mode, yaw_mode);
                 myled2=0;
-            /*--Dynamixel:end------------------------------------------------------------------*/       
-        
+            /*--Dynamixel:end------------------------------------------------------------------*/            
+                break;
+            
+            case 1: //ホームポジション
+                myled1=1; myled2=0; myled3=0; myled4=1; Dyna_home_position(); break;
+                
+            case 2: //リセット
+                myled1=0; myled2=1; myled3=1; myled4=0; Dyna_reset(); break;
+            
+            case 3: //終了
+                myled1=1; myled2=1; myled3=1; myled4=1; Dyna_end(); break;
+        }
+                        //現在の角度・電圧・電流を取得
+                myled1=1;
+                Dyna_GetData(Dyna_data);
+                myled1=0;
             /*--Thermal_Sensor:begin-----------------------------------------------------------*/
                 //値を取得
                 myled3=1;
@@ -321,18 +367,7 @@
                 myled4=1;
                 sprintf(Send_data,"%s%s%4.1f",Dyna_data,Thermo_data,CO2_data);
                 pc.printf("%s\n",Send_data);
-                myled4=0;           
-                break;
-            
-            case 1: //ホームポジション
-                myled1=1; myled2=0; myled3=0; myled4=1; Dyna_home_position(); break;
-                
-            case 2: //リセット
-                myled1=0; myled2=1; myled3=1; myled4=0; Dyna_reset(); break;
-            
-            case 3: //終了
-                myled1=1; myled2=1; myled3=1; myled4=1; Dyna_end(); break;
-        }
+                myled4=0;      
     }
 }