BE@R lab
SwitchMode
Dependencies: BEAR_Protocol InverseLeg iSerial mbed
Fork of
SwitchMode
by 
Arsapol Manamunchaiyaporn
Diff: main.cpp
- Revision:
- 7:3935c7dcc9c5
- Parent:
- 6:4afac9a87b60
- Child:
- 8:f17874479d80
--- a/main.cpp Wed Jan 27 12:54:11 2016 +0000
+++ b/main.cpp Thu Jan 28 15:23:43 2016 +0000
@@ -25,117 +25,128 @@
pc.printf("************Don't Forget to Save Data************\n");
while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
- sync_communicate.reset();
+ 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();
+ 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();
+ 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();
+ 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();
+ 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();
+ 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]);
+ pc.printf("*\t15) Ki : Right Knee [%f]\t\t*\n",temp[1]);
+ pc.printf("*\t16) Kd : Right Knee [%f]\t\t*\n",temp[2]);
while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
- sync_communicate.reset();
+ sync_communicate.reset();
bcom.getUpMargin(LEFT_SIDE,&temp[0]);
- pc.printf("*\t17) Set Left Hip Margin [%f]\n",temp[0]);
+ pc.printf("*\t17) Set Left Hip Margin [%f]\n",temp[0]);
while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
- sync_communicate.reset();
+ sync_communicate.reset();
bcom.getLowMargin(LEFT_SIDE,&temp[0]);
- pc.printf("*\t18) Set Left Knee Margin [%f]\n",temp[0]);
+ pc.printf("*\t18) Set Left Knee Margin [%f]\n",temp[0]);
while(sync_communicate.read_ms() < TIME_SYNC_COMMUNICATION);
- sync_communicate.reset();
+ 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();
+ 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();
+ 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();
+ 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();
+ 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();
+ 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();
+ 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();
+ 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();
+ 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]);
-
+ bcom.getUpAngleRange(RIGHT_SIDE,&temp[2],&temp[3]);
+ pc.printf("*\t27) Set Maximum Hip Angle Range \n");
+ pc.printf("\tLeft Maximum Hip Angle Range [%f]\n",temp[0]);
+ pc.printf("\tRight Maximum Hip Angle Range [%f]\n\n",temp[2]);
+
+ pc.printf("*\t28) Set Minimum Hip Angle Range \n");
+ pc.printf("\tLeft Minimum Hip Angle Range [%f]\n",temp[1]);
+ pc.printf("\tRight Minimum Hip Angle Range [%f]\n\n",temp[3]);
+
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]);
+ sync_communicate.reset();
+ bcom.getLowAngleRange(LEFT_SIDE,&temp[0],&temp[1]);
+ bcom.getLowAngleRange(RIGHT_SIDE,&temp[2],&temp[3]);
+ pc.printf("*\t29) Set Maximum Knee Angle Range \n");
+ pc.printf("\tLeft Maximum Knee Angle Range [%f]\n",temp[0]);
+ pc.printf("\tRight Maximum Knee Angle Range [%f]\n\n",temp[2]);
+
+ pc.printf("*\t30) Set Minimum Knee Angle Range \n");
+ pc.printf("\tLeft Minimum Knee Angle Range [%f]\n",temp[1]);
+ pc.printf("\tRight Minimum Knee Angle Range [%f]\n\n",temp[3]);
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)
- {
+ if(sync_communicate.read_ms() >0x7FFFFF00) {
sync_communicate.reset();
}
pc.printf("\n");
-
-
-
+
+
+
return option;
}
@@ -211,7 +222,7 @@
//Send Position to Motor
pc.printf("Input Degree : \n");
- pc.scanf("%f",&RH);
+ pc.scanf("%f",&temp);
if(temp!=9999) {
RH = temp;
pc.printf("Move Right Hip Motor to %f Degree\n",RH);
@@ -256,7 +267,7 @@
pc.printf("\nChange Kp of Left Hip to %f",temp);
bcom.setUpMotorKp(LEFT_SIDE,temp);
- } else bcom.saveDataToEEPROM(LEFT_SIDE,KP_UPPER_MOTOR);
+ } else bcom.saveDataToEEPROM(LEFT_SIDE,PID_UPPER_MOTOR);
} while(temp != 9999);
@@ -270,7 +281,7 @@
pc.printf("\nChange Ki of Left Hip to %f",temp);
bcom.setUpMotorKi(LEFT_SIDE,temp);
- } else bcom.saveDataToEEPROM(LEFT_SIDE,KI_UPPER_MOTOR);
+ } else bcom.saveDataToEEPROM(LEFT_SIDE,PID_UPPER_MOTOR);
} while(temp != 9999);
@@ -284,7 +295,7 @@
pc.printf("\nChange Kd of Left Hip to %f",temp);
bcom.setUpMotorKd(LEFT_SIDE,temp);
- } else bcom.saveDataToEEPROM(LEFT_SIDE,KD_UPPER_MOTOR);
+ } else bcom.saveDataToEEPROM(LEFT_SIDE,PID_UPPER_MOTOR);
} while(temp != 9999);
@@ -298,7 +309,7 @@
pc.printf("\nChange Kp of Left Knee to %f",temp);
bcom.setLowMotorKp(LEFT_SIDE,temp);
- } else bcom.saveDataToEEPROM(LEFT_SIDE,KP_LOWER_MOTOR);
+ } else bcom.saveDataToEEPROM(LEFT_SIDE,PID_LOWER_MOTOR);
} while(temp != 9999);
@@ -312,7 +323,7 @@
pc.printf("\nChange Ki of Left Knee to %f",temp);
bcom.setLowMotorKi(LEFT_SIDE,temp);
- } else bcom.saveDataToEEPROM(LEFT_SIDE,KI_LOWER_MOTOR);
+ } else bcom.saveDataToEEPROM(LEFT_SIDE,PID_LOWER_MOTOR);
} while(temp != 9999);
@@ -326,7 +337,7 @@
pc.printf("\nChange Kd of Left Knee to %f",temp);
bcom.setLowMotorKd(LEFT_SIDE,temp);
- } else bcom.saveDataToEEPROM(LEFT_SIDE,KD_LOWER_MOTOR);
+ } else bcom.saveDataToEEPROM(LEFT_SIDE,PID_LOWER_MOTOR);
} while(temp != 9999);
@@ -341,7 +352,7 @@
pc.printf("\nChange Kp of Right Hip to %f",temp);
bcom.setUpMotorKp(RIGHT_SIDE,temp);
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,KP_UPPER_MOTOR);
+ } else bcom.saveDataToEEPROM(RIGHT_SIDE,PID_UPPER_MOTOR);
} while(temp != 9999);
@@ -355,7 +366,7 @@
pc.printf("\nChange Ki of Right Hip to %f",temp);
bcom.setUpMotorKi(RIGHT_SIDE,temp);
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,KI_UPPER_MOTOR);
+ } else bcom.saveDataToEEPROM(RIGHT_SIDE,PID_UPPER_MOTOR);
} while(temp != 9999);
@@ -369,7 +380,7 @@
pc.printf("\nChange Kd of Right Hip to %f",temp);
bcom.setUpMotorKd(RIGHT_SIDE,temp);
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,KD_UPPER_MOTOR);
+ } else bcom.saveDataToEEPROM(RIGHT_SIDE,PID_UPPER_MOTOR);
} while(temp != 9999);
@@ -384,7 +395,7 @@
pc.printf("\nChange Kp of Right Knee to %f",temp);
bcom.setLowMotorKp(RIGHT_SIDE,temp);
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,KP_LOWER_MOTOR);
+ } else bcom.saveDataToEEPROM(RIGHT_SIDE,PID_LOWER_MOTOR);
} while(temp != 9999);
@@ -398,7 +409,7 @@
pc.printf("\nChange Ki of Right Knee to %f Degree\n",temp);
bcom.setLowMotorKi(RIGHT_SIDE,temp);
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,KI_LOWER_MOTOR);
+ } else bcom.saveDataToEEPROM(RIGHT_SIDE,PID_LOWER_MOTOR);
} while(temp != 9999);
@@ -412,7 +423,7 @@
pc.printf("\nChange Kd of Right Knee to %f",temp);
bcom.setLowMotorKd(RIGHT_SIDE,temp);
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,KD_LOWER_MOTOR);
+ } else bcom.saveDataToEEPROM(RIGHT_SIDE,PID_LOWER_MOTOR);
} while(temp != 9999);
@@ -425,7 +436,7 @@
if(temp != 9999) {
pc.printf("\nChange Left Hip Margin to %f\n",temp);
bcom.setUpMargin(LEFT_SIDE,temp);
- } //else
+ } //else
bcom.saveDataToEEPROM(LEFT_SIDE,UP_MARGIN);
}
@@ -437,7 +448,7 @@
if(temp != 9999) {
pc.printf("\nChange Left Knee Margin to %f\n",temp);
bcom.setLowMargin(LEFT_SIDE,temp);
- } //else
+ } //else
bcom.saveDataToEEPROM(LEFT_SIDE,LOW_MARGIN);
}
@@ -449,7 +460,8 @@
if(temp != 9999) {
pc.printf("\nChange Right Hip Margin to %f\n",temp);
bcom.setUpMargin(RIGHT_SIDE,temp);
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,UP_MARGIN);
+ } //else
+ bcom.saveDataToEEPROM(RIGHT_SIDE,UP_MARGIN);
}
else if(option == 20) { //Right Knee Margin
@@ -460,7 +472,8 @@
if(temp != 9999) {
pc.printf("\nChange Right Knee Margin to %f\n",temp);
bcom.setLowMargin(RIGHT_SIDE,temp);
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_MARGIN);
+ } //else
+ bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_MARGIN);
}
else if(option == 21) { //Lenght of Left Link Hip
@@ -473,7 +486,8 @@
LLink0 = temp;
a = true;
bcom.setUpLinkLength(LEFT_SIDE,temp);
- } else bcom.saveDataToEEPROM(LEFT_SIDE,UP_LINK_LENGTH);
+ } //else
+ bcom.saveDataToEEPROM(LEFT_SIDE,UP_LINK_LENGTH);
}
else if(option == 22) { //Lenght of Left Link Knee
@@ -486,7 +500,8 @@
LLink1 = temp;
b = true;
bcom.setLowLinkLength(LEFT_SIDE,temp);
- } else bcom.saveDataToEEPROM(LEFT_SIDE,LOW_LINK_LENGTH);
+ } //else
+ bcom.saveDataToEEPROM(LEFT_SIDE,LOW_LINK_LENGTH);
}
else if(option == 23) { //Lenght of Right Link Hip
@@ -499,7 +514,8 @@
RLink0 = temp;
c = true;
bcom.setUpLinkLength(RIGHT_SIDE,temp);
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,UP_LINK_LENGTH);
+ } //else
+ bcom.saveDataToEEPROM(RIGHT_SIDE,UP_LINK_LENGTH);
}
else if(option == 24) { //Lenght of Right Link Knee
@@ -512,13 +528,15 @@
RLink1 = temp;
d = true;
bcom.setLowLinkLength(RIGHT_SIDE,temp);
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_LINK_LENGTH);
+ } //else
+ bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_LINK_LENGTH);
}
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);
wait_ms(90);
bcom.getMotorPos(RIGHT_SIDE,&RHipAngle,&RKneeAngle);
@@ -552,7 +570,7 @@
} else {
pc.printf("\nYou have to do choice 21-23 first\n\n");
- wait(2);
+ wait(1);
}
}
@@ -580,7 +598,8 @@
pc.printf("\nMaximum Hip Angle Range of Left Side = %f\n",temp);
bcom.setUpAngleRange(LEFT_SIDE,temp,Lmin.Hip);
Lmax.Hip = temp;
- } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_UP);
+ } //else
+ bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_UP);
} else if(temp==2) { //Right
pc.printf("Input Maximum Hip Angle Range of Right Side\n");
@@ -590,7 +609,8 @@
pc.printf("\nMaximum Hip Angle Range of Right Side = %f\n",temp);
bcom.setUpAngleRange(RIGHT_SIDE,temp,Rmin.Hip);
Rmax.Hip = temp;
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_UP);
+ } //else
+ bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_UP);
}
} while(temp!=9999);
}
@@ -608,9 +628,10 @@
if(temp != 9999) {
pc.printf("\nMinumum Hip Angle Range of Left Side = %f\n",temp);
- bcom.setLowAngleRange(LEFT_SIDE,Lmax.Hip,temp);
+ bcom.setUpAngleRange(LEFT_SIDE,Lmax.Hip,temp);
Lmin.Hip = temp;
- } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_UP);
+ } //else
+ bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_UP);
} else if(temp==2) { //Right
pc.printf("Input Minumum Hip Angle Range of Right Side\n");
@@ -618,9 +639,10 @@
if(temp != 9999) {
pc.printf("\nMinumum Hip Angle Range of Right Side = %f\n",temp);
- bcom.setLowAngleRange(RIGHT_SIDE,Rmax.Hip,temp);
+ bcom.setUpAngleRange(RIGHT_SIDE,Rmax.Hip,temp);
Rmin.Hip = temp;
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_UP);
+ } //else
+ bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_UP);
}
} while(temp!=9999);
}
@@ -641,7 +663,8 @@
pc.printf("\nMaximum Knee Angle Range of Left Side = %f\n",temp);
bcom.setLowAngleRange(LEFT_SIDE,temp,Lmin.Knee);
Lmax.Knee = temp;
- } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_LOW);
+ } //else
+ bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_LOW);
} else if(temp==2) { //Right
pc.printf("Input Maximum Knee Angle Range of Right Side\n");
@@ -651,7 +674,8 @@
pc.printf("\nMaximum Knee Angle Range of Right Side = %f\n",temp);
bcom.setLowAngleRange(RIGHT_SIDE,temp,Rmin.Knee);
Rmax.Knee = temp;
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_LOW);
+ } //else
+ bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_LOW);
}
} while(temp!=9999);
}
@@ -671,7 +695,8 @@
pc.printf("\nMinumum Knee Angle Range of Left Side = %f\n",temp);
bcom.setLowAngleRange(LEFT_SIDE,Lmax.Knee,temp);
Lmin.Knee = temp;
- } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_LOW);
+ } //else
+ bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_LOW);
} else if(temp==2) { //Right
pc.printf("Input Minumum Knee Angle Range of Right Side\n");
@@ -681,7 +706,8 @@
pc.printf("\nMinumum Knee Angle Range of Right Side = %f\n",temp);
bcom.setLowAngleRange(RIGHT_SIDE,Rmax.Knee,temp);
Rmin.Knee = temp;
- } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_LOW);
+ } //else
+ bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_LOW);
}
} while(temp!=9999);
}
@@ -718,9 +744,9 @@
{
pc.baud(115200);
pc.printf("Start\n");
-
+
sync_communicate.start();
-
+
if(!button) {
while(!button);
SwMode();