BE@R lab
Stabilizer
Dependencies: BEAR_Protocol mbed Stabilizer iSerial
Fork of
MPU9250AHRS
by 
BE@R lab
Diff: main.cpp
- Revision:
- 16:b5b9827dd5dc
- Parent:
- 14:8101a48eb972
- Child:
- 17:51ac4252fa1a
--- a/main.cpp Wed Dec 23 11:21:43 2015 +0000
+++ b/main.cpp Wed Jan 20 01:54:44 2016 +0000
@@ -30,15 +30,363 @@
//InterruptIn event(PC_13);
DigitalIn enable(PC_13);
-//DigitalIn button(USER_BUTTON);
+DigitalIn button(USER_BUTTON);
+
+void SwMode()
+{
+ int option; // user's entered option will be saved in this variable
+ float kp1,kp2,kp3,kp4;
+ float ki1,ki2,ki3,ki4;
+ float kd1,kd2,kd3,kd4;
+ float LH,LK,RH,RK;
+ float temp_LH=0,temp_LK=0,temp_RH=0,temp_RK=0;
+ do { // do-while loop starts here.that display menu again and again until user select to exit program
+ // Displaying Options for the menu
+ 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");
+ 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 Knee data\t\t\t*\n");
+ pc.printf("*\t21) Save Dog Stand Position \t\t*\n");
+ pc.printf("*\t22) Save Dog Sit Position \t\t*\n");
+ pc.printf("*\t23) 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 {
+ pc.printf("\n1) Input Degree\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ //Send Position to Motor
+ pc.printf("Input Degree : \n");
+ pc.scanf("%f",&LH);
+ pc.printf("Move Left Hip Motor to %f Degree\n",LH);
+
+ L.setMotorPos(0x01,LH,temp_LK);
+ temp_LH = LH;
+
+ } else if(option == 2) {
+ /********************Save Data*********************/
+ }
+ } while(option != 2);
+
+ } else if(option == 2) { //Left Knee
+ do {
+ pc.printf("\n1) Input Degree\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ //Send Position to Motor
+ pc.printf("Input Degree : \n");
+ pc.scanf("%f",&LK);
+ pc.printf("Move Left Knee Motor to %f Degree\n",LK);
+
+ L.setMotorPos(0x01,temp_LH,LK);
+ temp_LK = LK;
+
+ } else if(option == 2) {
+ /********************Save Data*********************/
+ }
+ } while(option != 2);
+
+ } else if(option == 3) { //Right Hip
+ do {
+ pc.printf("\n1) Input Degree\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ //Send Position to Motor
+ pc.printf("Input Degree : \n");
+ pc.scanf("%f",&RH);
+ pc.printf("Move Right Hip Motor to %f Degree\n",RH);
+
+ R.setMotorPos(0x02,RH,temp_RK);
+ temp_RH = RH;
+
+ } else if(option == 2) {
+ /********************Save Data*********************/
+ }
+ } while(option != 2);
+
+ } else if(option == 4) { //Right Knee
+ do {
+ pc.printf("\n1) Input Degree\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ //Send Position to Motor
+ pc.printf("Input Degree : \n");
+ pc.scanf("%f",&RK);
+ pc.printf("Move Right Knee Motor to %f Degree\n",RK);
+
+ R.setMotorPos(0x02,temp_RH,RK);
+ temp_RK = RK;
+
+ } else if(option == 2) {
+ /********************Save Data*********************/
+ }
+ } while(option != 2);
+
+ } else if(option == 5) { //Left Hip
+ do {
+ pc.printf("\n1) Input Kp of Left Hip\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&kp1);
+ pc.printf("\nChange Kp of Left Hip to %f",kp1);
+
+ L.setUpMotorKp(0x01,kp1);
+ }
+ } while(option != 2);
+
+ } else if(option == 6) { //Left Hip
+ do {
+ pc.printf("\n1) Input Ki of Left Hip\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&ki1);
+ pc.printf("\nChange Ki of Left Hip to %f",ki1);
+
+ L.setUpMotorKi(0x01,ki1);
+ }
+ } while(option != 2);
+
+ } else if(option == 7) { //Left Hip
+ do {
+ pc.printf("\n1) Input Kd of Left Hip\n");
+ pc.printf("2) Main Menu\n");
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&kd1);
+ pc.printf("\nChange Kd of Left Hip to %f",kd1);
+
+ L.setUpMotorKd(0x01,kd1);
+ }
+ } while(option != 2);
+
+ } else if(option == 8) {
+ /*******************************Save Left Hip data*************************************/
-void UI()
-{
+ } else if(option == 9) { //Left Knee
+ do {
+ pc.printf("\n1) Input Kp of Left Knee\n");
+ pc.printf("2) Main Menu\n");
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&kp2);
+ pc.printf("\nChange Kp of Left Knee to %f",kp2);
+
+ L.setLowMotorKp(0x01,kp2);
+ }
+ } while(option != 2);
+
+ } else if(option == 10) { //Left Knee
+ do {
+ pc.printf("\n1) Input Ki of Left Knee\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&ki2);
+ pc.printf("\nChange Ki of Left Knee to %f",ki2);
+
+ L.setLowMotorKi(0x01,ki2);
+ }
+ } while(option != 2);
+
+ } else if(option == 11) { //Left Knee
+ do {
+ pc.printf("\n1) Input Kd of Left Knee\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&kd2);
+ pc.printf("\nChange Kd of Left Knee to %f",kd2);
+
+ L.setLowMotorKd(0x01,kd2);
+ }
+ } while(option != 2);
+
+
+ } else if(option == 12) {
+ /*******************************Save Left Knee data*************************************/
+
+
+ } else if(option == 13) { //Right Hip
+ do {
+ pc.printf("\n1) Input Kp of Right Hip\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&kp3);
+ pc.printf("\nChange Kp of Right Hip to %f",kp3);
+
+ R.setUpMotorKp(0x02,kp3);
+ }
+ } while(option != 2);
+
+ } else if(option == 14) { //Right Hip
+ do {
+ pc.printf("\n1) Input Ki of Right Hip\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&ki3);
+ pc.printf("\nChange Ki of Right Hip to %f",ki3);
+
+ R.setUpMotorKi(0x02,ki3);
+ }
+ } while(option != 2);
+
+ } else if(option == 15) { //Right Hip
+ do {
+ pc.printf("\n1) Input Kd of Right Hip\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&kd3);
+ pc.printf("\nChange Kd of Right Hip to %f",kd3);
+
+ R.setUpMotorKd(0x02,kd3);
+ }
+ } while(option != 2);
+
+
+ } else if(option == 16) {
+ /*******************************Save Right Hip data*************************************/
+
+
+ } else if(option == 17) { //Right Knee
+ do {
+ pc.printf("\n1) Input Kp of Right Knee\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&kp4);
+ pc.printf("\nChange Kp of Right Knee to %f",kp4);
+
+ R.setLowMotorKp(0x02,kp4);
+ }
+
+ } while(option != 2);
+ } else if(option == 18) { //Right Knee
+ do {
+ pc.printf("\n1) Input Ki of Right Knee\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&ki4);
+ pc.printf("\nChange Ki of Right Knee to %f Degree\n",ki4);
+
+ R.setLowMotorKi(0x02,ki4);
+ }
+ } while(option != 2);
+
+ } else if(option == 19) { //Right Knee
+ do {
+ pc.printf("\n1) Input Kd of Right Knee\n");
+ pc.printf("2) Main Menu\n");
+
+ pc.scanf("%d",&option);
+
+ if(option == 1) {
+ InputK(&kd4);
+ pc.printf("\nChange Kd of Right Knee to %f",kd4);
+
+ R.setLowMotorKd(0x02,kd4);
+ }
+ } while(option != 2);
+
+ } else if(option == 20) {
+ /*******************************Save Right Knee data*************************************/
+
+
+ } else if(option == 21) {
+ pc.printf("Save Dog Stand Position\n");
+
+ /*************************************Save Data to EEPROM***************************/
+
+ } else if(option == 22) {
+ pc.printf("Save Dog Sit Position\n");
+
+ /*************************************Save Data to EEPROM***************************/
+
+ } else if(option == 23) {
+ pc.printf("Are You Sure ?\n");
+ pc.printf("\n1) Yes\n");
+ pc.printf("2) No\n");
+
+ pc.scanf("%d",&option);
+
+ if(option==1) {
+ option = 23;
+ pc.printf("Please Push Button Restart\n");
+ } else pc.printf("Return to Main Menu\n");
+
+ }
+
+
+ else { // if user has entered invalid choice (other than 1,2,3 or 4)
+ // Displaying error message
+ pc.printf("Invalid Option entered\n");
+ }
+ } while(option != 23); // condition of do-while loop
}
void WheelChair()
{
+
//Start Here
//Stabilize.set_Body_Lenght(5);
Stabilize.set_current_zeta(roll);
@@ -72,7 +420,8 @@
int main()
{
pc.baud(115200);
-
+
+ if(button==1) SwMode();
/*while(1){
Kinematic test('P',10,15,10,10);