BE@R lab
SwitchMode
Dependencies: BEAR_Protocol InverseLeg iSerial mbed
Fork of
SwitchMode
by 
Arsapol Manamunchaiyaporn
Diff: main.cpp
- Revision:
- 6:4afac9a87b60
- Parent:
- 5:1e868598c7db
- Child:
- 7:3935c7dcc9c5
--- a/main.cpp Sat Jan 23 03:36:48 2016 +0000
+++ b/main.cpp Wed Jan 27 12:54:11 2016 +0000
@@ -1,7 +1,12 @@
+//#define ANDANTE_DEBUG
+
#include "string"
#include "BEAR_Protocol.h"
+
#include "Kinematic.h"
+#define TIME_SYNC_COMMUNICATION 5 //unit ms
+
Serial pc(SERIAL_TX,SERIAL_RX);
Bear_Communicate bcom(PA_15,PB_7,1000000);
Kinematic Left(0,0),Right(0,0);
@@ -11,12 +16,135 @@
#define LEFT_SIDE 0x01
#define RIGHT_SIDE 0x02
+Timer sync_communicate;
+
+int16_t show_menu()
+{
+ int16_t option=9999;
+ float temp[6];
+ pc.printf("************Don't Forget to Save Data************\n");
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getMotorPos(LEFT_SIDE,&temp[0],&temp[1]);
+ pc.printf("*\t1) Motor Left Hip [%f]\t\t*\n",temp[0]);
+ pc.printf("*\t2) Motor Left Knee [%f]\t\t*\n",temp[1]);
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getMotorPos(RIGHT_SIDE,&temp[0],&temp[1]);
+ pc.printf("*\t3) Motor Right Hip [%f]\t\t*\n",temp[0]);
+ pc.printf("*\t4) Motor Right Knee [%f]\t\t*\n",temp[1]);
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getUpMotorKpKiKd(LEFT_SIDE,&temp[0],&temp[1],&temp[2]);
+ pc.printf("*\t5) Kp : Left Hip [%f]\t\t*\n",temp[0]);
+ pc.printf("*\t6) Ki : Left Hip [%f]\t\t*\n",temp[1]);
+ pc.printf("*\t7) Kd : Left Hip [%f]\t\t*\n",temp[2]);
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getLowMotorKpKiKd(LEFT_SIDE,&temp[0],&temp[1],&temp[2]);
+ pc.printf("*\t8) Kp : Left Knee [%f]\t\t*\n",temp[0]);
+ pc.printf("*\t9) Ki : Left Knee [%f]\t\t*\n",temp[1]);
+ pc.printf("*\t10) Kd : Left Knee [%f]\t\t*\n",temp[2]);
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getUpMotorKpKiKd(RIGHT_SIDE,&temp[0],&temp[1],&temp[2]);
+ pc.printf("*\t11) Kp : Right Hip [%f]\t\t*\n",temp[0]);
+ pc.printf("*\t12) Ki : Right Hip [%f]\t\t*\n",temp[1]);
+ pc.printf("*\t13) Kd : Right Hip [%f]\t\t*\n",temp[2]);
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getLowMotorKpKiKd(RIGHT_SIDE,&temp[0],&temp[1],&temp[2]);
+ pc.printf("*\t14) Kp : Right Knee [%f]\t\t*\n",temp[0]);
+ pc.printf("*\t15) Ki : Right Knee [%f]\t\t*\n",temp[0]);
+ pc.printf("*\t16) Kd : Right Knee [%f]\t\t*\n",temp[0]);
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getUpMargin(LEFT_SIDE,&temp[0]);
+ pc.printf("*\t17) Set Left Hip Margin [%f]\n",temp[0]);
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getLowMargin(LEFT_SIDE,&temp[0]);
+ pc.printf("*\t18) Set Left Knee Margin [%f]\n",temp[0]);
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getUpMargin(RIGHT_SIDE,&temp[0]);
+ pc.printf("*\t19) Set Right Hip Margin [%f]\n",temp[0]);
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getLowMargin(RIGHT_SIDE,&temp[0]);
+ pc.printf("*\t20) Set Right Knee Margin [%f]\n",temp[0]);
+
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getUpLinkLength(LEFT_SIDE,&temp[0]);
+ pc.printf("*\t21) Set Lenght of Left Link Hip [%f]\n",temp[0]);
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getLowLinkLength(LEFT_SIDE,&temp[0]);
+ pc.printf("*\t22) Set Lenght of Left Link Knee [%f]\n",temp[0]);
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getUpLinkLength(RIGHT_SIDE,&temp[0]);
+ pc.printf("*\t23) Set Lenght of Right Link Hip [%f]\n",temp[0]);
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getLowLinkLength(RIGHT_SIDE,&temp[0]);
+ pc.printf("*\t24) Set Lenght of Right Link Knee [%f]\n",temp[0]);
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getOffset(LEFT_SIDE,&temp[0],&temp[1]);
+ pc.printf("*\t25) Set Offset x[%f] y[%f]\n",temp[0],temp[1]);
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getBodyWidth(LEFT_SIDE,&temp[0]);
+ pc.printf("*\t26) Set Body Width [%f]\n",temp[0]);
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getUpAngleRange(LEFT_SIDE,&temp[0],&temp[1]);
+ pc.printf("*\t27) Set Maximum Hip Angle Range [%f]\n",temp[0]);
+ pc.printf("*\t28) Set Minimum Hip Angle Range [%f]\n",temp[1]);
+
+ while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
+ sync_communicate.reset();
+ bcom.getUpAngleRange(RIGHT_SIDE,&temp[0],&temp[1]);
+ pc.printf("*\t29) Set Maximum Knee Angle Range [%f]\n",temp[0]);
+ pc.printf("*\t30) Set Minimum Knee Angle Range [%f]\n",temp[1]);
+
+ pc.printf("*\t40) Exit Program \t\t\t*\n");
+ pc.printf("************Don't Forget to Save Data************\n");
+ // Prompting user to enter an option according to menu
+ pc.printf("Please select an option : ");
+
+
+ pc.scanf("%d",&option);
+ if(sync_communicate.read_ms() >0x7FFFFF00)
+ {
+ sync_communicate.reset();
+ }
+ pc.printf("\n");
+
+
+
+ return option;
+}
+
void SwMode()
{
int option;
float temp;
float LH,LK,RH,RK;
- float LLink[2],RLink[2];
+ //float LLink[2],RLink[2];
float LLink0,LLink1,RLink0,RLink1;
struct max_ang {
float Hip;
@@ -33,55 +161,10 @@
a = b = c = d = false;
float temp_LH=0,temp_LK=0,temp_RH=0,temp_RK=0;
do {
- pc.printf("************Don't Forget to Save Data************\n");
- pc.printf("*\t1) Motor Left Hip \t\t\t*\n");
- 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");
+ //float buff;
- 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");
+ option = show_menu();
- pc.printf("*\t40) Exit Program \t\t\t*\n");
- pc.printf("************Don't Forget to Save Data************\n");
- // Prompting user to enter an option according to menu
- pc.printf("Please select an option : ");
-
- pc.scanf("%d",&option);
-
- pc.printf("\n");
if(option == 1)// Left Hip
do {
@@ -97,7 +180,7 @@
temp_LH = LH;
/********************Save Data*********************/
- } else bcom.saveDataToEEPROM(LEFT_SIDE,MOTOR_UPPER_ANG);
+ } //else bcom.saveDataToEEPROM(LEFT_SIDE,MOTOR_UPPER_ANG);
} while(temp != 9999);
@@ -117,7 +200,7 @@
/********************Save Data*********************/
- } else bcom.saveDataToEEPROM(LEFT_SIDE,MOTOR_LOWER_ANG);
+ } //else bcom.saveDataToEEPROM(LEFT_SIDE,MOTOR_LOWER_ANG);
} while(temp != 9999);
@@ -138,7 +221,7 @@
/********************Save Data*********************/
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,MOTOR_UPPER_ANG);
+ } //else bcom.saveDataToEEPROM(RIGHT_SIDE,MOTOR_UPPER_ANG);
} while(temp != 9999);
@@ -159,7 +242,7 @@
/********************Save Data*********************/
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,MOTOR_LOWER_ANG);
+ } //else bcom.saveDataToEEPROM(RIGHT_SIDE,MOTOR_LOWER_ANG);
} while(temp != 9999);
@@ -342,7 +425,8 @@
if(temp != 9999) {
pc.printf("\nChange Left Hip Margin to %f\n",temp);
bcom.setUpMargin(LEFT_SIDE,temp);
- } else bcom.saveDataToEEPROM(LEFT_SIDE,UP_MARGIN);
+ } //else
+ bcom.saveDataToEEPROM(LEFT_SIDE,UP_MARGIN);
}
else if(option == 18) { //Left Knee Margin
@@ -353,7 +437,8 @@
if(temp != 9999) {
pc.printf("\nChange Left Knee Margin to %f\n",temp);
bcom.setLowMargin(LEFT_SIDE,temp);
- } else bcom.saveDataToEEPROM(LEFT_SIDE,LOW_MARGIN);
+ } //else
+ bcom.saveDataToEEPROM(LEFT_SIDE,LOW_MARGIN);
}
else if(option == 19) { //Right Hip Margin
@@ -443,12 +528,12 @@
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);
-
+
Left.ForwardKinematicCalculation();
Right.ForwardKinematicCalculation();
@@ -462,7 +547,7 @@
offset_Z = z1-z2;
bcom.setOffset(LEFT_SIDE,offset_Y,offset_Z);
-
+
bcom.saveDataToEEPROM(LEFT_SIDE,OFFSET);
} else {
@@ -627,11 +712,15 @@
}
+
+
int main()
{
pc.baud(115200);
pc.printf("Start\n");
-
+
+ sync_communicate.start();
+
if(!button) {
while(!button);
SwMode();
@@ -639,6 +728,8 @@
}
+
+
/*int main()
{
pc.baud(105200);