SwitchMode
Dependencies: BEAR_Protocol InverseLeg iSerial mbed
Revision 3:b7a8a60085c1, committed 2016-01-23
- Comitter:
- icyzkungz
- Date:
- Sat Jan 23 01:21:22 2016 +0000
- Parent:
- 2:d6be1c49d9d5
- Commit message:
- solve kinematic problem in offset
Changed in this revision
--- a/BEAR_Protocol.lib Fri Jan 22 08:04:41 2016 +0000 +++ b/BEAR_Protocol.lib Sat Jan 23 01:21:22 2016 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/teams/BEaR-lab/code/BEAR_Protocol/#2398eeafa967 +http://developer.mbed.org/teams/BEaR-lab/code/BEAR_Protocol/#b34eababcf56
--- a/InverseLeg.lib Fri Jan 22 08:04:41 2016 +0000 +++ b/InverseLeg.lib Sat Jan 23 01:21:22 2016 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/teams/BEaR-lab/code/InverseLeg/#d3bb9ead2a55 +http://mbed.org/teams/BEaR-lab/code/InverseLeg/#a2a5ebd65f46
--- a/main.cpp Fri Jan 22 08:04:41 2016 +0000 +++ b/main.cpp Sat Jan 23 01:21:22 2016 +0000 @@ -4,10 +4,11 @@ Serial pc(SERIAL_TX,SERIAL_RX); Bear_Communicate bcom(PA_15,PB_7,115200); +Kinematic Left(0,0),Right(0,0); DigitalIn button(USER_BUTTON); -#define LEFT_SIDE 0x01 +#define LEFT_SIDE 0x01 #define RIGHT_SIDE 0x02 void SwMode() @@ -16,6 +17,7 @@ float temp; float LH,LK,RH,RK; float LLink[2],RLink[2]; + float LLink0,LLink1,RLink0,RLink1; struct max_ang { float Hip; float Knee; @@ -51,18 +53,21 @@ 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 Knee data\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 Offset\n"); - pc.printf("*\t26) Set Body Lenght of Left Side\n"); - pc.printf("*\t27) Set Body Lenght of Right Side\n"); - pc.printf("*\t28) Set Maximum Hip Angle Range\n"); - pc.printf("*\t29) Set Minimum Hip Angle Range\n"); - pc.printf("*\t30) Set Maximum Knee Angle Range\n"); - pc.printf("*\t31) Set Minimum Knee Angle Range\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("*\t40) Exit Program \t\t\t*\n"); pc.printf("************Don't Forget to Save Data************\n"); @@ -386,64 +391,117 @@ } else bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_MARGIN); } - else if(option == 25) { //Offset - if(a == true && b == true && c == true && d == true) { - //float HipAngle[2],KneeAngle[2]; - float LHipAngle,LKneeAngle; - float RHipAngle,RKneeAngle; - bcom.getMotorPos(LEFT_SIDE,&LHipAngle,&LKneeAngle); - bcom.getMotorPos(RIGHT_SIDE,&RHipAngle,&RKneeAngle); + else if(option == 25) { //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); - Kinematic L( 'Z', LLink[0], LLink[1], LHipAngle, LKneeAngle); - Kinematic R( 'Z', RLink[0], RLink[1], RHipAngle, RKneeAngle); + if(temp != 9999) { + pc.printf("\nLenght of Left Link Hip = %f\n",temp); + LLink0 = temp; + a = true; + bcom.setUpLinkLength(LEFT_SIDE,temp); + } else bcom.saveDataToEEPROM(LEFT_SIDE,UP_LINK_LENGTH); + } + + else if(option == 26) { //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); - L.ForwardKinematicCalculation(); - R.ForwardKinematicCalculation(); + if(temp != 9999) { + pc.printf("\nLenght of Left Link Knee = %f\n",temp); + LLink1 = temp; + b = true; + bcom.setLowLinkLength(LEFT_SIDE,temp); + } else bcom.saveDataToEEPROM(LEFT_SIDE,LOW_LINK_LENGTH); + } + + else if(option == 27) { //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); - float offset_Y,offset_Z; - offset_Y = L.get_Position_Y()-R.get_Position_Y(); - offset_Z = L.get_Position_Z()-R.get_Position_Z(); + if(temp != 9999) { + pc.printf("\nLenght of Right Link Hip = %f\n",temp); + RLink0 = temp; + c = true; + bcom.setUpLinkLength(RIGHT_SIDE,temp); + } else bcom.saveDataToEEPROM(RIGHT_SIDE,UP_LINK_LENGTH); + } - bcom.setOffset(LEFT_SIDE,offset_Y,offset_Z); - bcom.saveDataToEEPROM(LEFT_SIDE,OFFSET); + else if(option == 28) { //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); - } else pc.printf("\nYou have to do choice 25-28 first\n\n"); + if(temp != 9999) { + pc.printf("\nLenght of Right Link Knee = %f\n",temp); + RLink1 = temp; + d = true; + bcom.setLowLinkLength(RIGHT_SIDE,temp); + } else bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_LINK_LENGTH); } - else if(option == 26) { //Body Lenght of Left Side - do { - pc.printf("\nType 9999 to Exit to Main Menu\n"); - pc.printf("Input Body Lenght of Left Side\n"); - pc.scanf("%f",&temp); + else if(option == 29) { //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"); - if(temp != 9999) { - pc.printf("\nBody Lenght = %f\n",temp); - bcom.setBodyLength(LEFT_SIDE,temp); - } else bcom.saveDataToEEPROM(LEFT_SIDE,BODY_LENGTH); + pc.printf("Kinematic"); + Left.set_Link_Hip(LLink0); + Left.set_Link_Knee(LLink1); + Left.set_Zeta_Hip(LHipAngle); + Left.set_Zeta_Knee(LKneeAngle); + + Right.set_Link_Hip(RLink0); + 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"); - } while(temp!=9999); + float offset_Y,offset_Z; + float y1,y2,z1,z2; + y1 = Left.get_Position_Y(); + y2 = Right.get_Position_Y(); + z1 = Left.get_Position_Z(); + z2 = Right.get_Position_Z(); + offset_Y = y1-y2; + 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"); + wait(2); + } } - else if(option == 27) { //Body Lenght of Right Side - do { - pc.printf("\nType 9999 to Exit to Main Menu\n"); - pc.printf("Input Body Lenght of Right Side\n"); - pc.scanf("%f",&temp); - - if(temp != 9999) { - pc.printf("\nBody Lenght = %f\n",temp); - bcom.setBodyLength(RIGHT_SIDE,temp); - } else bcom.saveDataToEEPROM(RIGHT_SIDE,BODY_LENGTH); - - } while(temp!=9999); + else if(option == 30) { //setBodyWidth + pc.printf("\nType 9999 to Exit to Main Menu\n"); + pc.printf("Input Body Width\n"); + pc.scanf("%f",&temp); + pc.printf("\nBody Lenght = %f\n",temp); + bcom.setBodyWidth(LEFT_SIDE,temp); + bcom.saveDataToEEPROM(LEFT_SIDE,BODY_WIDTH); } - else if(option == 28) { //Set Maximum Hip Angle Range + else if(option == 31) { //Set Maximum Hip Angle Range do { pc.printf("\nType 9999 to Exit to Main Menu\n"); pc.printf("1) Left Side\n"); pc.printf("2) Right Side\n"); - pc.scanf("%f\n",&temp); + pc.scanf("%f",&temp); if(temp==1) { //Left pc.printf("Input Maximum Hip Angle Range of Left Side\n"); @@ -468,12 +526,12 @@ } while(temp!=9999); } - else if(option == 29) { //Set Minumum Hip Angle Range + else if(option == 32) { //Set Minumum Hip Angle Range do { pc.printf("\nType 9999 to Exit to Main Menu\n"); pc.printf("1) Left Side\n"); pc.printf("2) Right Side\n"); - pc.scanf("%f\n",&temp); + pc.scanf("%f",&temp); if(temp==1) { //Left pc.printf("Input Minumum Hip Angle Range of Left Side\n"); @@ -499,12 +557,12 @@ } - else if(option == 30) { //Set Maximum Knee Angle Range + else if(option == 33) { //Set Maximum Knee Angle Range do { pc.printf("\nType 9999 to Exit to Main Menu\n"); pc.printf("1) Left Side\n"); pc.printf("2) Right Side\n"); - pc.scanf("%f\n",&temp); + pc.scanf("%f",&temp); if(temp==1) { //Left pc.printf("Input Maximum Knee Angle Range of Left Side\n"); @@ -529,12 +587,12 @@ } while(temp!=9999); } - else if(option == 28) { //Set Minumum Knee Angle Range + else if(option == 34) { //Set Minumum Knee Angle Range do { pc.printf("\nType 9999 to Exit to Main Menu\n"); pc.printf("1) Left Side\n"); pc.printf("2) Right Side\n"); - pc.scanf("%f\n",&temp); + pc.scanf("%f",&temp); if(temp==1) { //Left pc.printf("Input Minumum Knee Angle Range of Left Side\n");