Diff: main.cpp

Revision:

4:16cadf26dbab

Parent:

3:b7a8a60085c1

Child:

5:1e868598c7db

--- a/main.cpp	Sat Jan 23 01:21:22 2016 +0000
+++ b/main.cpp	Sat Jan 23 03:29:55 2016 +0000
@@ -3,7 +3,7 @@
 #include "Kinematic.h"
 
 Serial pc(SERIAL_TX,SERIAL_RX);
-Bear_Communicate bcom(PA_15,PB_7,115200);
+Bear_Communicate bcom(PA_15,PB_7,1000000);
 Kinematic Left(0,0),Right(0,0);
 
 DigitalIn button(USER_BUTTON);
@@ -38,36 +38,41 @@
         pc.printf("*\t2) Motor Left Knee \t\t\t*\n");
         pc.printf("*\t3) Motor Right Hip \t\t\t*\n");
         pc.printf("*\t4) Motor Right Knee \t\t\t*\n");
+        
         pc.printf("*\t5) Kp : Left Hip \t\t\t*\n");
         pc.printf("*\t6) Ki : Left Hip \t\t\t*\n");
         pc.printf("*\t7) Kd : Left Hip \t\t\t*\n");
-        pc.printf("*\t8) Save Left Hip(p,i,d) data\t\t*\n");
-        pc.printf("*\t9) Kp : Left Knee \t\t\t*\n");
-        pc.printf("*\t10) Ki : Left Knee \t\t\t*\n");
-        pc.printf("*\t11) Kd : Left Knee \t\t\t*\n");
-        pc.printf("*\t12) Save Left Knee(p,i,d) data\t\t*\n");
-        pc.printf("*\t13) Kp : Right Hip \t\t\t*\n");
-        pc.printf("*\t14) Ki : Right Hip \t\t\t*\n");
-        pc.printf("*\t15) Kd : Right Hip \t\t\t*\n");
-        pc.printf("*\t16) Save Right Hip(p,i,d) data\t\t*\n");
-        pc.printf("*\t17) Kp : Right Knee \t\t\t*\n");
-        pc.printf("*\t18) Ki : Right Knee \t\t\t*\n");
-        pc.printf("*\t19) Kd : Right Knee \t\t\t*\n");
-        pc.printf("*\t20) Save Right K(p,i,d) data\t\t\t*\n");
-        pc.printf("*\t21) Set Left Hip Margin\n");
-        pc.printf("*\t22) Set Left Knee Margin\n");
-        pc.printf("*\t23) Set Right Hip Margin\n");
-        pc.printf("*\t24) Set Right Knee Margin\n");
-        pc.printf("*\t25) Set Lenght of Left Link Hip\n");
-        pc.printf("*\t26) Set Lenght of Left Link Knee\n");
-        pc.printf("*\t27) Set Lenght of Right Link Hip\n");
-        pc.printf("*\t28) Set Lenght of Right Link Knee\n");
-        pc.printf("*\t29) Set Offset\n");
-        pc.printf("*\t30) Set Body Width\n");
-        pc.printf("*\t31) Set Maximum Hip Angle Range\n");
-        pc.printf("*\t32) Set Minimum Hip Angle Range\n");
-        pc.printf("*\t33) Set Maximum Knee Angle Range\n");
-        pc.printf("*\t34) Set Minimum Knee Angle Range\n");
+
+        pc.printf("*\t8) Kp : Left Knee \t\t\t*\n");
+        pc.printf("*\t9) Ki : Left Knee \t\t\t*\n");
+        pc.printf("*\t10) Kd : Left Knee \t\t\t*\n");
+        
+        pc.printf("*\t11) Kp : Right Hip \t\t\t*\n");
+        pc.printf("*\t12) Ki : Right Hip \t\t\t*\n");
+        pc.printf("*\t13) Kd : Right Hip \t\t\t*\n");
+        
+        pc.printf("*\t14) Kp : Right Knee \t\t\t*\n");
+        pc.printf("*\t15) Ki : Right Knee \t\t\t*\n");
+        pc.printf("*\t16) Kd : Right Knee \t\t\t*\n");
+        
+        pc.printf("*\t17) Set Left Hip Margin\n");
+        pc.printf("*\t18) Set Left Knee Margin\n");
+        pc.printf("*\t19) Set Right Hip Margin\n");
+        pc.printf("*\t20) Set Right Knee Margin\n");
+        
+        pc.printf("*\t21) Set Lenght of Left Link Hip\n");
+        pc.printf("*\t22) Set Lenght of Left Link Knee\n");
+        pc.printf("*\t23) Set Lenght of Right Link Hip\n");
+        pc.printf("*\t24) Set Lenght of Right Link Knee\n");
+        
+        pc.printf("*\t25) Set Offset\n");
+        
+        pc.printf("*\t26) Set Body Width\n");
+        
+        pc.printf("*\t27) Set Maximum Hip Angle Range\n");
+        pc.printf("*\t28) Set Minimum Hip Angle Range\n");
+        pc.printf("*\t29) Set Maximum Knee Angle Range\n");
+        pc.printf("*\t30) Set Minimum Knee Angle Range\n");
 
         pc.printf("*\t40) Exit Program \t\t\t*\n");
         pc.printf("************Don't Forget to Save Data************\n");
@@ -200,11 +205,7 @@
 
             } while(temp != 9999);
 
-        } else if(option == 8) {
-            pc.printf("\nType 9999 to Exit to Main Menu\n");
-            /*******************************Save Left Hip data*************************************/
-
-        } else if(option == 9) { //Left Knee
+        } else if(option == 8) { //Left Knee
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("\nInput Kp of Left Knee\n");
@@ -218,7 +219,7 @@
 
             } while(temp != 9999);
 
-        } else if(option == 10) { //Left Knee
+        } else if(option == 9) { //Left Knee
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("\nInput Ki of Left Knee\n");
@@ -232,7 +233,7 @@
 
             } while(temp != 9999);
 
-        } else if(option == 11) { //Left Knee
+        } else if(option == 10) { //Left Knee
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("\nInput Kd of Left Knee\n");
@@ -247,12 +248,7 @@
             } while(temp != 9999);
 
 
-        } else if(option == 12) {
-            pc.printf("\nType 9999 to Exit to Main Menu\n");
-            /*******************************Save Left Knee data*************************************/
-
-
-        } else if(option == 13) { //Right Hip
+        } else if(option == 11) { //Right Hip
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("\nInput Kp of Right Hip\n");
@@ -266,7 +262,7 @@
 
             } while(temp != 9999);
 
-        } else if(option == 14) { //Right Hip
+        } else if(option == 12) { //Right Hip
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("\nInput Ki of Right Hip\n");
@@ -280,7 +276,7 @@
 
             } while(temp != 9999);
 
-        } else if(option == 15) { //Right Hip
+        } else if(option == 13) { //Right Hip
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("\nInput Kd of Right Hip\n");
@@ -295,12 +291,7 @@
             } while(temp != 9999);
 
 
-        } else if(option == 16) {
-            pc.printf("\nType 9999 to Exit to Main Menu\n");
-            /*******************************Save Right Hip data*************************************/
-
-
-        } else if(option == 17) { //Right Knee
+        } else if(option == 14) { //Right Knee
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("\nInput Kp of Right Knee\n");
@@ -314,7 +305,7 @@
 
             } while(temp != 9999);
 
-        } else if(option == 18) { //Right Knee
+        } else if(option == 15) { //Right Knee
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("\nInput Ki of Right Knee\n");
@@ -328,7 +319,7 @@
 
             } while(temp != 9999);
 
-        } else if(option == 19) { //Right Knee
+        } else if(option == 16) { //Right Knee
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("\nInput Kd of Right Knee\n");
@@ -342,12 +333,8 @@
 
             } while(temp != 9999);
 
-        } else if(option == 20) {
-            pc.printf("\nType 9999 to Exit to Main Menu\n");
-            /*******************************Save Right Knee data*************************************/
 
-
-        } else if(option == 21) { //Left Hip Margin
+        } else if(option == 17) { //Left Hip Margin
             pc.printf("\nType 9999 to Exit to Main Menu\n");
             pc.printf("Input Margin\n");
             pc.scanf("%f",&temp);
@@ -358,7 +345,7 @@
             } else bcom.saveDataToEEPROM(LEFT_SIDE,UP_MARGIN);
         }
 
-        else if(option == 22) { //Left Knee Margin
+        else if(option == 18) { //Left Knee Margin
             pc.printf("\nType 9999 to Exit to Main Menu\n");
             pc.printf("Input Margin\n");
             pc.scanf("%f",&temp);
@@ -369,7 +356,7 @@
             } else bcom.saveDataToEEPROM(LEFT_SIDE,LOW_MARGIN);
         }
 
-        else if(option == 23) { //Right Hip Margin
+        else if(option == 19) { //Right Hip Margin
             pc.printf("\nType 9999 to Exit to Main Menu\n");
             pc.printf("Input Margin\n");
             pc.scanf("%f",&temp);
@@ -380,7 +367,7 @@
             } else bcom.saveDataToEEPROM(RIGHT_SIDE,UP_MARGIN);
         }
 
-        else if(option == 24) { //Right Knee Margin
+        else if(option == 20) { //Right Knee Margin
             pc.printf("\nType 9999 to Exit to Main Menu\n");
             pc.printf("Input Margin\n");
             pc.scanf("%f",&temp);
@@ -391,7 +378,7 @@
             } else bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_MARGIN);
         }
 
-        else if(option == 25) { //Lenght of Left Link Hip
+        else if(option == 21) { //Lenght of Left Link Hip
             pc.printf("\nType 9999 to Exit to Main Menu\n");
             pc.printf("Input Lenght of Left Link Hip\n");
             pc.scanf("%f",&temp);
@@ -404,7 +391,7 @@
             } else bcom.saveDataToEEPROM(LEFT_SIDE,UP_LINK_LENGTH);
         }
 
-        else if(option == 26) { //Lenght of Left Link Knee
+        else if(option == 22) { //Lenght of Left Link Knee
             pc.printf("\nType 9999 to Exit to Main Menu\n");
             pc.printf("Input Lenght of Left Link Knee\n");
             pc.scanf("%f",&temp);
@@ -417,7 +404,7 @@
             } else bcom.saveDataToEEPROM(LEFT_SIDE,LOW_LINK_LENGTH);
         }
 
-        else if(option == 27) { //Lenght of Right Link Hip
+        else if(option == 23) { //Lenght of Right Link Hip
             pc.printf("\nType 9999 to Exit to Main Menu\n");
             pc.printf("Input Lenght of Right Link Hip\n");
             pc.scanf("%f",&temp);
@@ -430,7 +417,7 @@
             } else bcom.saveDataToEEPROM(RIGHT_SIDE,UP_LINK_LENGTH);
         }
 
-        else if(option == 28) { //Lenght of Right Link Knee
+        else if(option == 24) { //Lenght of Right Link Knee
             pc.printf("\nType 9999 to Exit to Main Menu\n");
             pc.printf("Input Lenght of Right Link Knee\n");
             pc.scanf("%f",&temp);
@@ -443,16 +430,15 @@
             } else bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_LINK_LENGTH);
         }
 
-        else if(option == 29) { //Offset
+        else if(option == 25) { //Offset
             if(a == true && b == true && c == true && d == true) {
                 float LHipAngle,LKneeAngle;
                 float RHipAngle,RKneeAngle;
-                //bcom.getMotorPos(LEFT_SIDE,&LHipAngle,&LKneeAngle);
-                pc.printf("1\n");
-                //bcom.getMotorPos(RIGHT_SIDE,&RHipAngle,&RKneeAngle);
-                pc.printf("2\n");
+                bcom.getMotorPos(LEFT_SIDE,&LHipAngle,&LKneeAngle);
+                wait_ms(90);
+                bcom.getMotorPos(RIGHT_SIDE,&RHipAngle,&RKneeAngle);
+                wait_ms(90);
 
-                pc.printf("Kinematic");
                 Left.set_Link_Hip(LLink0);
                 Left.set_Link_Knee(LLink1);
                 Left.set_Zeta_Hip(LHipAngle);
@@ -462,11 +448,6 @@
                 Right.set_Link_Knee(RLink1);
                 Right.set_Zeta_Hip(RHipAngle);
                 Right.set_Zeta_Knee(RKneeAngle);
-                //LHipAngle = RHipAngle = LKneeAngle = RKneeAngle = 20; //Test Recieve Value
-                //Kinematic Left('Z',LLink0,LLink1,LHipAngle,LKneeAngle);
-                pc.printf("Obj1\n");
-                //Kinematic Right('Z',RLink0,RLink1,RHipAngle,RKneeAngle);
-                pc.printf("End Kinematic\n");
 
                 float offset_Y,offset_Z;
                 float y1,y2,z1,z2;
@@ -478,16 +459,16 @@
                 offset_Z = z1-z2;
 
                 bcom.setOffset(LEFT_SIDE,offset_Y,offset_Z);
-                pc.printf("3\n");
+                
                 bcom.saveDataToEEPROM(LEFT_SIDE,OFFSET);
 
             } else {
-                pc.printf("\nYou have to do choice 25-27 first\n\n");
+                pc.printf("\nYou have to do choice 21-23 first\n\n");
                 wait(2);
             }
         }
 
-        else if(option == 30) { //setBodyWidth
+        else if(option == 26) { //setBodyWidth
             pc.printf("\nType 9999 to Exit to Main Menu\n");
             pc.printf("Input Body Width\n");
             pc.scanf("%f",&temp);
@@ -496,7 +477,7 @@
             bcom.saveDataToEEPROM(LEFT_SIDE,BODY_WIDTH);
         }
 
-        else if(option == 31) { //Set Maximum Hip Angle Range
+        else if(option == 27) { //Set Maximum Hip Angle Range
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("1) Left Side\n");
@@ -526,7 +507,7 @@
             } while(temp!=9999);
         }
 
-        else if(option == 32) { //Set Minumum Hip Angle Range
+        else if(option == 28) { //Set Minumum Hip Angle Range
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("1) Left Side\n");
@@ -557,7 +538,7 @@
         }
 
 
-        else if(option == 33) { //Set Maximum Knee Angle Range
+        else if(option == 29) { //Set Maximum Knee Angle Range
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("1) Left Side\n");
@@ -587,7 +568,7 @@
             } while(temp!=9999);
         }
 
-        else if(option == 34) { //Set Minumum Knee Angle Range
+        else if(option == 30) { //Set Minumum Knee Angle Range
             do {
                 pc.printf("\nType 9999 to Exit to Main Menu\n");
                 pc.printf("1) Left Side\n");
@@ -657,7 +638,7 @@
 
 /*int main()
 {
-    pc.baud(115200);
+    pc.baud(105200);
     int num;
     do {
         //float num;