BE@R lab / Mbed 2 deprecated SwitchMode

SwitchMode

Dependencies:   BEAR_Protocol InverseLeg iSerial mbed

Fork of SwitchMode by Arsapol Manamunchaiyaporn

main.cpp@5:1e868598c7db, 2016-01-23 (annotated)

Committer:
icyzkungz
Date:
Sat Jan 23 03:36:48 2016 +0000
Revision:
5:1e868598c7db
Parent:
4:16cadf26dbab
Child:
6:4afac9a87b60
add forward kinematic in offset

Who changed what in which revision?

UserRevisionLine numberNew contents of line
icyzkungz 0:f73c80b36c20 1 #include "string"
icyzkungz 0:f73c80b36c20 2 #include "BEAR_Protocol.h"
icyzkungz 0:f73c80b36c20 3 #include "Kinematic.h"
icyzkungz 0:f73c80b36c20 4
icyzkungz 0:f73c80b36c20 5 Serial pc(SERIAL_TX,SERIAL_RX);
icyzkungz 4:16cadf26dbab 6 Bear_Communicate bcom(PA_15,PB_7,1000000);
icyzkungz 3:b7a8a60085c1 7 Kinematic Left(0,0),Right(0,0);
icyzkungz 0:f73c80b36c20 8
icyzkungz 0:f73c80b36c20 9 DigitalIn button(USER_BUTTON);
icyzkungz 0:f73c80b36c20 10
icyzkungz 3:b7a8a60085c1 11 #define LEFT_SIDE 0x01
icyzkungz 0:f73c80b36c20 12 #define RIGHT_SIDE 0x02
icyzkungz 0:f73c80b36c20 13
icyzkungz 0:f73c80b36c20 14 void SwMode()
icyzkungz 0:f73c80b36c20 15 {
icyzkungz 2:d6be1c49d9d5 16 int option;
icyzkungz 0:f73c80b36c20 17 float temp;
icyzkungz 0:f73c80b36c20 18 float LH,LK,RH,RK;
icyzkungz 0:f73c80b36c20 19 float LLink[2],RLink[2];
icyzkungz 3:b7a8a60085c1 20 float LLink0,LLink1,RLink0,RLink1;
icyzkungz 0:f73c80b36c20 21 struct max_ang {
icyzkungz 0:f73c80b36c20 22 float Hip;
icyzkungz 0:f73c80b36c20 23 float Knee;
icyzkungz 0:f73c80b36c20 24 };
icyzkungz 0:f73c80b36c20 25 struct min_ang {
icyzkungz 0:f73c80b36c20 26 float Hip;
icyzkungz 0:f73c80b36c20 27 float Knee;
icyzkungz 0:f73c80b36c20 28 };
icyzkungz 0:f73c80b36c20 29 max_ang Lmax,Lmin;
icyzkungz 0:f73c80b36c20 30 min_ang Rmax,Rmin;
icyzkungz 1:183e6088a1fa 31 Lmax.Knee = Lmax.Hip = Lmin.Knee = Lmin.Hip = Rmax.Knee = Rmax.Hip = Rmin.Knee = Rmin.Hip = 0;
icyzkungz 0:f73c80b36c20 32 bool a,b,c,d;
icyzkungz 0:f73c80b36c20 33 a = b = c = d = false;
icyzkungz 0:f73c80b36c20 34 float temp_LH=0,temp_LK=0,temp_RH=0,temp_RK=0;
icyzkungz 2:d6be1c49d9d5 35 do {
icyzkungz 0:f73c80b36c20 36 pc.printf("************Don't Forget to Save Data************\n");
icyzkungz 0:f73c80b36c20 37 pc.printf("*\t1) Motor Left Hip \t\t\t*\n");
icyzkungz 0:f73c80b36c20 38 pc.printf("*\t2) Motor Left Knee \t\t\t*\n");
icyzkungz 0:f73c80b36c20 39 pc.printf("*\t3) Motor Right Hip \t\t\t*\n");
icyzkungz 0:f73c80b36c20 40 pc.printf("*\t4) Motor Right Knee \t\t\t*\n");
icyzkungz 4:16cadf26dbab 41
icyzkungz 0:f73c80b36c20 42 pc.printf("*\t5) Kp : Left Hip \t\t\t*\n");
icyzkungz 0:f73c80b36c20 43 pc.printf("*\t6) Ki : Left Hip \t\t\t*\n");
icyzkungz 0:f73c80b36c20 44 pc.printf("*\t7) Kd : Left Hip \t\t\t*\n");
icyzkungz 4:16cadf26dbab 45
icyzkungz 4:16cadf26dbab 46 pc.printf("*\t8) Kp : Left Knee \t\t\t*\n");
icyzkungz 4:16cadf26dbab 47 pc.printf("*\t9) Ki : Left Knee \t\t\t*\n");
icyzkungz 4:16cadf26dbab 48 pc.printf("*\t10) Kd : Left Knee \t\t\t*\n");
icyzkungz 4:16cadf26dbab 49
icyzkungz 4:16cadf26dbab 50 pc.printf("*\t11) Kp : Right Hip \t\t\t*\n");
icyzkungz 4:16cadf26dbab 51 pc.printf("*\t12) Ki : Right Hip \t\t\t*\n");
icyzkungz 4:16cadf26dbab 52 pc.printf("*\t13) Kd : Right Hip \t\t\t*\n");
icyzkungz 4:16cadf26dbab 53
icyzkungz 4:16cadf26dbab 54 pc.printf("*\t14) Kp : Right Knee \t\t\t*\n");
icyzkungz 4:16cadf26dbab 55 pc.printf("*\t15) Ki : Right Knee \t\t\t*\n");
icyzkungz 4:16cadf26dbab 56 pc.printf("*\t16) Kd : Right Knee \t\t\t*\n");
icyzkungz 4:16cadf26dbab 57
icyzkungz 4:16cadf26dbab 58 pc.printf("*\t17) Set Left Hip Margin\n");
icyzkungz 4:16cadf26dbab 59 pc.printf("*\t18) Set Left Knee Margin\n");
icyzkungz 4:16cadf26dbab 60 pc.printf("*\t19) Set Right Hip Margin\n");
icyzkungz 4:16cadf26dbab 61 pc.printf("*\t20) Set Right Knee Margin\n");
icyzkungz 4:16cadf26dbab 62
icyzkungz 4:16cadf26dbab 63 pc.printf("*\t21) Set Lenght of Left Link Hip\n");
icyzkungz 4:16cadf26dbab 64 pc.printf("*\t22) Set Lenght of Left Link Knee\n");
icyzkungz 4:16cadf26dbab 65 pc.printf("*\t23) Set Lenght of Right Link Hip\n");
icyzkungz 4:16cadf26dbab 66 pc.printf("*\t24) Set Lenght of Right Link Knee\n");
icyzkungz 4:16cadf26dbab 67
icyzkungz 4:16cadf26dbab 68 pc.printf("*\t25) Set Offset\n");
icyzkungz 4:16cadf26dbab 69
icyzkungz 4:16cadf26dbab 70 pc.printf("*\t26) Set Body Width\n");
icyzkungz 4:16cadf26dbab 71
icyzkungz 4:16cadf26dbab 72 pc.printf("*\t27) Set Maximum Hip Angle Range\n");
icyzkungz 4:16cadf26dbab 73 pc.printf("*\t28) Set Minimum Hip Angle Range\n");
icyzkungz 4:16cadf26dbab 74 pc.printf("*\t29) Set Maximum Knee Angle Range\n");
icyzkungz 4:16cadf26dbab 75 pc.printf("*\t30) Set Minimum Knee Angle Range\n");
icyzkungz 0:f73c80b36c20 76
icyzkungz 0:f73c80b36c20 77 pc.printf("*\t40) Exit Program \t\t\t*\n");
icyzkungz 0:f73c80b36c20 78 pc.printf("************Don't Forget to Save Data************\n");
icyzkungz 0:f73c80b36c20 79 // Prompting user to enter an option according to menu
icyzkungz 0:f73c80b36c20 80 pc.printf("Please select an option : ");
icyzkungz 0:f73c80b36c20 81
icyzkungz 0:f73c80b36c20 82 pc.scanf("%d",&option);
icyzkungz 0:f73c80b36c20 83
icyzkungz 0:f73c80b36c20 84 pc.printf("\n");
icyzkungz 0:f73c80b36c20 85
icyzkungz 0:f73c80b36c20 86 if(option == 1)// Left Hip
icyzkungz 0:f73c80b36c20 87 do {
icyzkungz 0:f73c80b36c20 88 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 89 //Send Position to Motor
icyzkungz 0:f73c80b36c20 90 pc.printf("Input Degree : \n");
icyzkungz 0:f73c80b36c20 91 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 92 if(temp!=9999) {
icyzkungz 0:f73c80b36c20 93 LH = temp;
icyzkungz 0:f73c80b36c20 94 pc.printf("Move Left Hip Motor to %f Degree\n",LH);
icyzkungz 0:f73c80b36c20 95
icyzkungz 0:f73c80b36c20 96 bcom.setMotorPos(LEFT_SIDE,LH,temp_LK);
icyzkungz 0:f73c80b36c20 97 temp_LH = LH;
icyzkungz 0:f73c80b36c20 98
icyzkungz 0:f73c80b36c20 99 /********************Save Data*********************/
icyzkungz 0:f73c80b36c20 100 } else bcom.saveDataToEEPROM(LEFT_SIDE,MOTOR_UPPER_ANG);
icyzkungz 0:f73c80b36c20 101
icyzkungz 0:f73c80b36c20 102 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 103
icyzkungz 0:f73c80b36c20 104 else if(option == 2) //Left Knee
icyzkungz 0:f73c80b36c20 105 do {
icyzkungz 0:f73c80b36c20 106 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 107
icyzkungz 0:f73c80b36c20 108 //Send Position to Motor
icyzkungz 0:f73c80b36c20 109 pc.printf("Input Degree : \n");
icyzkungz 0:f73c80b36c20 110 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 111 if(temp!=9999) {
icyzkungz 0:f73c80b36c20 112 LK = temp;
icyzkungz 0:f73c80b36c20 113 pc.printf("Move Left Knee Motor to %f Degree\n",LK);
icyzkungz 0:f73c80b36c20 114
icyzkungz 0:f73c80b36c20 115 bcom.setMotorPos(LEFT_SIDE,temp_LH,LK);
icyzkungz 0:f73c80b36c20 116 temp_LK = LK;
icyzkungz 0:f73c80b36c20 117
icyzkungz 0:f73c80b36c20 118
icyzkungz 0:f73c80b36c20 119 /********************Save Data*********************/
icyzkungz 0:f73c80b36c20 120 } else bcom.saveDataToEEPROM(LEFT_SIDE,MOTOR_LOWER_ANG);
icyzkungz 0:f73c80b36c20 121
icyzkungz 0:f73c80b36c20 122 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 123
icyzkungz 0:f73c80b36c20 124
icyzkungz 0:f73c80b36c20 125 else if(option == 3) { //Right Hip
icyzkungz 0:f73c80b36c20 126 do {
icyzkungz 0:f73c80b36c20 127 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 128
icyzkungz 0:f73c80b36c20 129 //Send Position to Motor
icyzkungz 0:f73c80b36c20 130 pc.printf("Input Degree : \n");
icyzkungz 0:f73c80b36c20 131 pc.scanf("%f",&RH);
icyzkungz 0:f73c80b36c20 132 if(temp!=9999) {
icyzkungz 0:f73c80b36c20 133 RH = temp;
icyzkungz 0:f73c80b36c20 134 pc.printf("Move Right Hip Motor to %f Degree\n",RH);
icyzkungz 0:f73c80b36c20 135
icyzkungz 0:f73c80b36c20 136 bcom.setMotorPos(RIGHT_SIDE,RH,temp_RK);
icyzkungz 0:f73c80b36c20 137 temp_RH = RH;
icyzkungz 0:f73c80b36c20 138
icyzkungz 0:f73c80b36c20 139
icyzkungz 0:f73c80b36c20 140 /********************Save Data*********************/
icyzkungz 0:f73c80b36c20 141 } else bcom.saveDataToEEPROM(RIGHT_SIDE,MOTOR_UPPER_ANG);
icyzkungz 0:f73c80b36c20 142
icyzkungz 0:f73c80b36c20 143 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 144
icyzkungz 0:f73c80b36c20 145 } else if(option == 4) { //Right Knee
icyzkungz 0:f73c80b36c20 146 do {
icyzkungz 0:f73c80b36c20 147 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 148
icyzkungz 0:f73c80b36c20 149 //Send Position to Motor
icyzkungz 0:f73c80b36c20 150 pc.printf("Input Degree : \n");
icyzkungz 0:f73c80b36c20 151 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 152
icyzkungz 0:f73c80b36c20 153 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 154 RK = temp;
icyzkungz 0:f73c80b36c20 155 pc.printf("Move Right Knee Motor to %f Degree\n",RK);
icyzkungz 0:f73c80b36c20 156
icyzkungz 0:f73c80b36c20 157 bcom.setMotorPos(RIGHT_SIDE,temp_RH,RK);
icyzkungz 0:f73c80b36c20 158 temp_RK = RK;
icyzkungz 0:f73c80b36c20 159
icyzkungz 0:f73c80b36c20 160
icyzkungz 0:f73c80b36c20 161 /********************Save Data*********************/
icyzkungz 0:f73c80b36c20 162 } else bcom.saveDataToEEPROM(RIGHT_SIDE,MOTOR_LOWER_ANG);
icyzkungz 0:f73c80b36c20 163
icyzkungz 0:f73c80b36c20 164 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 165
icyzkungz 0:f73c80b36c20 166 } else if(option == 5) { //Left Hip
icyzkungz 0:f73c80b36c20 167 do {
icyzkungz 0:f73c80b36c20 168 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 169 pc.printf("\nInput Kp of Left Hip\n");
icyzkungz 0:f73c80b36c20 170 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 171
icyzkungz 0:f73c80b36c20 172 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 173 pc.printf("\nChange Kp of Left Hip to %f",temp);
icyzkungz 0:f73c80b36c20 174
icyzkungz 0:f73c80b36c20 175 bcom.setUpMotorKp(LEFT_SIDE,temp);
icyzkungz 0:f73c80b36c20 176 } else bcom.saveDataToEEPROM(LEFT_SIDE,KP_UPPER_MOTOR);
icyzkungz 0:f73c80b36c20 177
icyzkungz 0:f73c80b36c20 178 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 179
icyzkungz 0:f73c80b36c20 180 } else if(option == 6) { //Left Hip
icyzkungz 0:f73c80b36c20 181 do {
icyzkungz 0:f73c80b36c20 182 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 183 pc.printf("\nInput Ki of Left Hip\n");
icyzkungz 0:f73c80b36c20 184 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 185
icyzkungz 0:f73c80b36c20 186 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 187 pc.printf("\nChange Ki of Left Hip to %f",temp);
icyzkungz 0:f73c80b36c20 188
icyzkungz 0:f73c80b36c20 189 bcom.setUpMotorKi(LEFT_SIDE,temp);
icyzkungz 0:f73c80b36c20 190 } else bcom.saveDataToEEPROM(LEFT_SIDE,KI_UPPER_MOTOR);
icyzkungz 0:f73c80b36c20 191
icyzkungz 0:f73c80b36c20 192 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 193
icyzkungz 0:f73c80b36c20 194 } else if(option == 7) { //Left Hip
icyzkungz 0:f73c80b36c20 195 do {
icyzkungz 0:f73c80b36c20 196 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 197 pc.printf("\nInput Kd of Left Hip\n");
icyzkungz 0:f73c80b36c20 198 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 199
icyzkungz 0:f73c80b36c20 200 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 201 pc.printf("\nChange Kd of Left Hip to %f",temp);
icyzkungz 0:f73c80b36c20 202
icyzkungz 0:f73c80b36c20 203 bcom.setUpMotorKd(LEFT_SIDE,temp);
icyzkungz 0:f73c80b36c20 204 } else bcom.saveDataToEEPROM(LEFT_SIDE,KD_UPPER_MOTOR);
icyzkungz 0:f73c80b36c20 205
icyzkungz 0:f73c80b36c20 206 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 207
icyzkungz 4:16cadf26dbab 208 } else if(option == 8) { //Left Knee
icyzkungz 0:f73c80b36c20 209 do {
icyzkungz 0:f73c80b36c20 210 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 211 pc.printf("\nInput Kp of Left Knee\n");
icyzkungz 0:f73c80b36c20 212 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 213
icyzkungz 0:f73c80b36c20 214 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 215 pc.printf("\nChange Kp of Left Knee to %f",temp);
icyzkungz 0:f73c80b36c20 216
icyzkungz 0:f73c80b36c20 217 bcom.setLowMotorKp(LEFT_SIDE,temp);
icyzkungz 0:f73c80b36c20 218 } else bcom.saveDataToEEPROM(LEFT_SIDE,KP_LOWER_MOTOR);
icyzkungz 0:f73c80b36c20 219
icyzkungz 0:f73c80b36c20 220 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 221
icyzkungz 4:16cadf26dbab 222 } else if(option == 9) { //Left Knee
icyzkungz 0:f73c80b36c20 223 do {
icyzkungz 0:f73c80b36c20 224 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 225 pc.printf("\nInput Ki of Left Knee\n");
icyzkungz 0:f73c80b36c20 226 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 227
icyzkungz 0:f73c80b36c20 228 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 229 pc.printf("\nChange Ki of Left Knee to %f",temp);
icyzkungz 0:f73c80b36c20 230
icyzkungz 0:f73c80b36c20 231 bcom.setLowMotorKi(LEFT_SIDE,temp);
icyzkungz 0:f73c80b36c20 232 } else bcom.saveDataToEEPROM(LEFT_SIDE,KI_LOWER_MOTOR);
icyzkungz 0:f73c80b36c20 233
icyzkungz 0:f73c80b36c20 234 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 235
icyzkungz 4:16cadf26dbab 236 } else if(option == 10) { //Left Knee
icyzkungz 0:f73c80b36c20 237 do {
icyzkungz 0:f73c80b36c20 238 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 239 pc.printf("\nInput Kd of Left Knee\n");
icyzkungz 0:f73c80b36c20 240 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 241
icyzkungz 0:f73c80b36c20 242 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 243 pc.printf("\nChange Kd of Left Knee to %f",temp);
icyzkungz 0:f73c80b36c20 244
icyzkungz 0:f73c80b36c20 245 bcom.setLowMotorKd(LEFT_SIDE,temp);
icyzkungz 0:f73c80b36c20 246 } else bcom.saveDataToEEPROM(LEFT_SIDE,KD_LOWER_MOTOR);
icyzkungz 0:f73c80b36c20 247
icyzkungz 0:f73c80b36c20 248 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 249
icyzkungz 0:f73c80b36c20 250
icyzkungz 4:16cadf26dbab 251 } else if(option == 11) { //Right Hip
icyzkungz 0:f73c80b36c20 252 do {
icyzkungz 0:f73c80b36c20 253 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 254 pc.printf("\nInput Kp of Right Hip\n");
icyzkungz 0:f73c80b36c20 255 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 256
icyzkungz 0:f73c80b36c20 257 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 258 pc.printf("\nChange Kp of Right Hip to %f",temp);
icyzkungz 0:f73c80b36c20 259
icyzkungz 0:f73c80b36c20 260 bcom.setUpMotorKp(RIGHT_SIDE,temp);
icyzkungz 0:f73c80b36c20 261 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KP_UPPER_MOTOR);
icyzkungz 0:f73c80b36c20 262
icyzkungz 0:f73c80b36c20 263 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 264
icyzkungz 4:16cadf26dbab 265 } else if(option == 12) { //Right Hip
icyzkungz 0:f73c80b36c20 266 do {
icyzkungz 0:f73c80b36c20 267 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 268 pc.printf("\nInput Ki of Right Hip\n");
icyzkungz 0:f73c80b36c20 269 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 270
icyzkungz 0:f73c80b36c20 271 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 272 pc.printf("\nChange Ki of Right Hip to %f",temp);
icyzkungz 0:f73c80b36c20 273
icyzkungz 0:f73c80b36c20 274 bcom.setUpMotorKi(RIGHT_SIDE,temp);
icyzkungz 0:f73c80b36c20 275 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KI_UPPER_MOTOR);
icyzkungz 0:f73c80b36c20 276
icyzkungz 0:f73c80b36c20 277 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 278
icyzkungz 4:16cadf26dbab 279 } else if(option == 13) { //Right Hip
icyzkungz 0:f73c80b36c20 280 do {
icyzkungz 0:f73c80b36c20 281 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 282 pc.printf("\nInput Kd of Right Hip\n");
icyzkungz 0:f73c80b36c20 283 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 284
icyzkungz 0:f73c80b36c20 285 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 286 pc.printf("\nChange Kd of Right Hip to %f",temp);
icyzkungz 0:f73c80b36c20 287
icyzkungz 0:f73c80b36c20 288 bcom.setUpMotorKd(RIGHT_SIDE,temp);
icyzkungz 0:f73c80b36c20 289 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KD_UPPER_MOTOR);
icyzkungz 0:f73c80b36c20 290
icyzkungz 0:f73c80b36c20 291 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 292
icyzkungz 0:f73c80b36c20 293
icyzkungz 4:16cadf26dbab 294 } else if(option == 14) { //Right Knee
icyzkungz 0:f73c80b36c20 295 do {
icyzkungz 0:f73c80b36c20 296 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 297 pc.printf("\nInput Kp of Right Knee\n");
icyzkungz 0:f73c80b36c20 298 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 299
icyzkungz 0:f73c80b36c20 300 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 301 pc.printf("\nChange Kp of Right Knee to %f",temp);
icyzkungz 0:f73c80b36c20 302
icyzkungz 0:f73c80b36c20 303 bcom.setLowMotorKp(RIGHT_SIDE,temp);
icyzkungz 0:f73c80b36c20 304 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KP_LOWER_MOTOR);
icyzkungz 0:f73c80b36c20 305
icyzkungz 0:f73c80b36c20 306 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 307
icyzkungz 4:16cadf26dbab 308 } else if(option == 15) { //Right Knee
icyzkungz 0:f73c80b36c20 309 do {
icyzkungz 0:f73c80b36c20 310 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 311 pc.printf("\nInput Ki of Right Knee\n");
icyzkungz 0:f73c80b36c20 312 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 313
icyzkungz 0:f73c80b36c20 314 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 315 pc.printf("\nChange Ki of Right Knee to %f Degree\n",temp);
icyzkungz 0:f73c80b36c20 316
icyzkungz 0:f73c80b36c20 317 bcom.setLowMotorKi(RIGHT_SIDE,temp);
icyzkungz 0:f73c80b36c20 318 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KI_LOWER_MOTOR);
icyzkungz 0:f73c80b36c20 319
icyzkungz 0:f73c80b36c20 320 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 321
icyzkungz 4:16cadf26dbab 322 } else if(option == 16) { //Right Knee
icyzkungz 0:f73c80b36c20 323 do {
icyzkungz 0:f73c80b36c20 324 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 325 pc.printf("\nInput Kd of Right Knee\n");
icyzkungz 0:f73c80b36c20 326 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 327
icyzkungz 0:f73c80b36c20 328 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 329 pc.printf("\nChange Kd of Right Knee to %f",temp);
icyzkungz 0:f73c80b36c20 330
icyzkungz 0:f73c80b36c20 331 bcom.setLowMotorKd(RIGHT_SIDE,temp);
icyzkungz 0:f73c80b36c20 332 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KD_LOWER_MOTOR);
icyzkungz 0:f73c80b36c20 333
icyzkungz 0:f73c80b36c20 334 } while(temp != 9999);
icyzkungz 0:f73c80b36c20 335
icyzkungz 0:f73c80b36c20 336
icyzkungz 4:16cadf26dbab 337 } else if(option == 17) { //Left Hip Margin
icyzkungz 0:f73c80b36c20 338 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 339 pc.printf("Input Margin\n");
icyzkungz 0:f73c80b36c20 340 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 341
icyzkungz 0:f73c80b36c20 342 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 343 pc.printf("\nChange Left Hip Margin to %f\n",temp);
icyzkungz 0:f73c80b36c20 344 bcom.setUpMargin(LEFT_SIDE,temp);
icyzkungz 0:f73c80b36c20 345 } else bcom.saveDataToEEPROM(LEFT_SIDE,UP_MARGIN);
icyzkungz 0:f73c80b36c20 346 }
icyzkungz 0:f73c80b36c20 347
icyzkungz 4:16cadf26dbab 348 else if(option == 18) { //Left Knee Margin
icyzkungz 0:f73c80b36c20 349 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 350 pc.printf("Input Margin\n");
icyzkungz 0:f73c80b36c20 351 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 352
icyzkungz 0:f73c80b36c20 353 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 354 pc.printf("\nChange Left Knee Margin to %f\n",temp);
icyzkungz 0:f73c80b36c20 355 bcom.setLowMargin(LEFT_SIDE,temp);
icyzkungz 0:f73c80b36c20 356 } else bcom.saveDataToEEPROM(LEFT_SIDE,LOW_MARGIN);
icyzkungz 0:f73c80b36c20 357 }
icyzkungz 0:f73c80b36c20 358
icyzkungz 4:16cadf26dbab 359 else if(option == 19) { //Right Hip Margin
icyzkungz 0:f73c80b36c20 360 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 361 pc.printf("Input Margin\n");
icyzkungz 0:f73c80b36c20 362 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 363
icyzkungz 0:f73c80b36c20 364 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 365 pc.printf("\nChange Right Hip Margin to %f\n",temp);
icyzkungz 0:f73c80b36c20 366 bcom.setUpMargin(RIGHT_SIDE,temp);
icyzkungz 0:f73c80b36c20 367 } else bcom.saveDataToEEPROM(RIGHT_SIDE,UP_MARGIN);
icyzkungz 0:f73c80b36c20 368 }
icyzkungz 0:f73c80b36c20 369
icyzkungz 4:16cadf26dbab 370 else if(option == 20) { //Right Knee Margin
icyzkungz 0:f73c80b36c20 371 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 372 pc.printf("Input Margin\n");
icyzkungz 0:f73c80b36c20 373 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 374
icyzkungz 0:f73c80b36c20 375 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 376 pc.printf("\nChange Right Knee Margin to %f\n",temp);
icyzkungz 0:f73c80b36c20 377 bcom.setLowMargin(RIGHT_SIDE,temp);
icyzkungz 0:f73c80b36c20 378 } else bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_MARGIN);
icyzkungz 0:f73c80b36c20 379 }
icyzkungz 0:f73c80b36c20 380
icyzkungz 4:16cadf26dbab 381 else if(option == 21) { //Lenght of Left Link Hip
icyzkungz 3:b7a8a60085c1 382 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 3:b7a8a60085c1 383 pc.printf("Input Lenght of Left Link Hip\n");
icyzkungz 3:b7a8a60085c1 384 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 385
icyzkungz 3:b7a8a60085c1 386 if(temp != 9999) {
icyzkungz 3:b7a8a60085c1 387 pc.printf("\nLenght of Left Link Hip = %f\n",temp);
icyzkungz 3:b7a8a60085c1 388 LLink0 = temp;
icyzkungz 3:b7a8a60085c1 389 a = true;
icyzkungz 3:b7a8a60085c1 390 bcom.setUpLinkLength(LEFT_SIDE,temp);
icyzkungz 3:b7a8a60085c1 391 } else bcom.saveDataToEEPROM(LEFT_SIDE,UP_LINK_LENGTH);
icyzkungz 3:b7a8a60085c1 392 }
icyzkungz 3:b7a8a60085c1 393
icyzkungz 4:16cadf26dbab 394 else if(option == 22) { //Lenght of Left Link Knee
icyzkungz 3:b7a8a60085c1 395 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 3:b7a8a60085c1 396 pc.printf("Input Lenght of Left Link Knee\n");
icyzkungz 3:b7a8a60085c1 397 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 398
icyzkungz 3:b7a8a60085c1 399 if(temp != 9999) {
icyzkungz 3:b7a8a60085c1 400 pc.printf("\nLenght of Left Link Knee = %f\n",temp);
icyzkungz 3:b7a8a60085c1 401 LLink1 = temp;
icyzkungz 3:b7a8a60085c1 402 b = true;
icyzkungz 3:b7a8a60085c1 403 bcom.setLowLinkLength(LEFT_SIDE,temp);
icyzkungz 3:b7a8a60085c1 404 } else bcom.saveDataToEEPROM(LEFT_SIDE,LOW_LINK_LENGTH);
icyzkungz 3:b7a8a60085c1 405 }
icyzkungz 3:b7a8a60085c1 406
icyzkungz 4:16cadf26dbab 407 else if(option == 23) { //Lenght of Right Link Hip
icyzkungz 3:b7a8a60085c1 408 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 3:b7a8a60085c1 409 pc.printf("Input Lenght of Right Link Hip\n");
icyzkungz 3:b7a8a60085c1 410 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 411
icyzkungz 3:b7a8a60085c1 412 if(temp != 9999) {
icyzkungz 3:b7a8a60085c1 413 pc.printf("\nLenght of Right Link Hip = %f\n",temp);
icyzkungz 3:b7a8a60085c1 414 RLink0 = temp;
icyzkungz 3:b7a8a60085c1 415 c = true;
icyzkungz 3:b7a8a60085c1 416 bcom.setUpLinkLength(RIGHT_SIDE,temp);
icyzkungz 3:b7a8a60085c1 417 } else bcom.saveDataToEEPROM(RIGHT_SIDE,UP_LINK_LENGTH);
icyzkungz 3:b7a8a60085c1 418 }
icyzkungz 0:f73c80b36c20 419
icyzkungz 4:16cadf26dbab 420 else if(option == 24) { //Lenght of Right Link Knee
icyzkungz 3:b7a8a60085c1 421 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 3:b7a8a60085c1 422 pc.printf("Input Lenght of Right Link Knee\n");
icyzkungz 3:b7a8a60085c1 423 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 424
icyzkungz 3:b7a8a60085c1 425 if(temp != 9999) {
icyzkungz 3:b7a8a60085c1 426 pc.printf("\nLenght of Right Link Knee = %f\n",temp);
icyzkungz 3:b7a8a60085c1 427 RLink1 = temp;
icyzkungz 3:b7a8a60085c1 428 d = true;
icyzkungz 3:b7a8a60085c1 429 bcom.setLowLinkLength(RIGHT_SIDE,temp);
icyzkungz 3:b7a8a60085c1 430 } else bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_LINK_LENGTH);
icyzkungz 0:f73c80b36c20 431 }
icyzkungz 0:f73c80b36c20 432
icyzkungz 4:16cadf26dbab 433 else if(option == 25) { //Offset
icyzkungz 3:b7a8a60085c1 434 if(a == true && b == true && c == true && d == true) {
icyzkungz 3:b7a8a60085c1 435 float LHipAngle,LKneeAngle;
icyzkungz 3:b7a8a60085c1 436 float RHipAngle,RKneeAngle;
icyzkungz 4:16cadf26dbab 437 bcom.getMotorPos(LEFT_SIDE,&LHipAngle,&LKneeAngle);
icyzkungz 4:16cadf26dbab 438 wait_ms(90);
icyzkungz 4:16cadf26dbab 439 bcom.getMotorPos(RIGHT_SIDE,&RHipAngle,&RKneeAngle);
icyzkungz 4:16cadf26dbab 440 wait_ms(90);
icyzkungz 0:f73c80b36c20 441
icyzkungz 3:b7a8a60085c1 442 Left.set_Link_Hip(LLink0);
icyzkungz 3:b7a8a60085c1 443 Left.set_Link_Knee(LLink1);
icyzkungz 3:b7a8a60085c1 444 Left.set_Zeta_Hip(LHipAngle);
icyzkungz 3:b7a8a60085c1 445 Left.set_Zeta_Knee(LKneeAngle);
icyzkungz 3:b7a8a60085c1 446
icyzkungz 3:b7a8a60085c1 447 Right.set_Link_Hip(RLink0);
icyzkungz 3:b7a8a60085c1 448 Right.set_Link_Knee(RLink1);
icyzkungz 3:b7a8a60085c1 449 Right.set_Zeta_Hip(RHipAngle);
icyzkungz 3:b7a8a60085c1 450 Right.set_Zeta_Knee(RKneeAngle);
icyzkungz 5:1e868598c7db 451
icyzkungz 5:1e868598c7db 452 Left.ForwardKinematicCalculation();
icyzkungz 5:1e868598c7db 453 Right.ForwardKinematicCalculation();
icyzkungz 0:f73c80b36c20 454
icyzkungz 3:b7a8a60085c1 455 float offset_Y,offset_Z;
icyzkungz 3:b7a8a60085c1 456 float y1,y2,z1,z2;
icyzkungz 3:b7a8a60085c1 457 y1 = Left.get_Position_Y();
icyzkungz 3:b7a8a60085c1 458 y2 = Right.get_Position_Y();
icyzkungz 3:b7a8a60085c1 459 z1 = Left.get_Position_Z();
icyzkungz 3:b7a8a60085c1 460 z2 = Right.get_Position_Z();
icyzkungz 3:b7a8a60085c1 461 offset_Y = y1-y2;
icyzkungz 3:b7a8a60085c1 462 offset_Z = z1-z2;
icyzkungz 3:b7a8a60085c1 463
icyzkungz 3:b7a8a60085c1 464 bcom.setOffset(LEFT_SIDE,offset_Y,offset_Z);
icyzkungz 4:16cadf26dbab 465
icyzkungz 3:b7a8a60085c1 466 bcom.saveDataToEEPROM(LEFT_SIDE,OFFSET);
icyzkungz 3:b7a8a60085c1 467
icyzkungz 3:b7a8a60085c1 468 } else {
icyzkungz 4:16cadf26dbab 469 pc.printf("\nYou have to do choice 21-23 first\n\n");
icyzkungz 3:b7a8a60085c1 470 wait(2);
icyzkungz 3:b7a8a60085c1 471 }
icyzkungz 0:f73c80b36c20 472 }
icyzkungz 0:f73c80b36c20 473
icyzkungz 4:16cadf26dbab 474 else if(option == 26) { //setBodyWidth
icyzkungz 3:b7a8a60085c1 475 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 3:b7a8a60085c1 476 pc.printf("Input Body Width\n");
icyzkungz 3:b7a8a60085c1 477 pc.scanf("%f",&temp);
icyzkungz 3:b7a8a60085c1 478 pc.printf("\nBody Lenght = %f\n",temp);
icyzkungz 3:b7a8a60085c1 479 bcom.setBodyWidth(LEFT_SIDE,temp);
icyzkungz 3:b7a8a60085c1 480 bcom.saveDataToEEPROM(LEFT_SIDE,BODY_WIDTH);
icyzkungz 0:f73c80b36c20 481 }
icyzkungz 0:f73c80b36c20 482
icyzkungz 4:16cadf26dbab 483 else if(option == 27) { //Set Maximum Hip Angle Range
icyzkungz 0:f73c80b36c20 484 do {
icyzkungz 0:f73c80b36c20 485 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 486 pc.printf("1) Left Side\n");
icyzkungz 0:f73c80b36c20 487 pc.printf("2) Right Side\n");
icyzkungz 3:b7a8a60085c1 488 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 489
icyzkungz 0:f73c80b36c20 490 if(temp==1) { //Left
icyzkungz 0:f73c80b36c20 491 pc.printf("Input Maximum Hip Angle Range of Left Side\n");
icyzkungz 0:f73c80b36c20 492 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 493
icyzkungz 0:f73c80b36c20 494 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 495 pc.printf("\nMaximum Hip Angle Range of Left Side = %f\n",temp);
icyzkungz 0:f73c80b36c20 496 bcom.setUpAngleRange(LEFT_SIDE,temp,Lmin.Hip);
icyzkungz 0:f73c80b36c20 497 Lmax.Hip = temp;
icyzkungz 0:f73c80b36c20 498 } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_UP);
icyzkungz 0:f73c80b36c20 499
icyzkungz 0:f73c80b36c20 500 } else if(temp==2) { //Right
icyzkungz 0:f73c80b36c20 501 pc.printf("Input Maximum Hip Angle Range of Right Side\n");
icyzkungz 0:f73c80b36c20 502 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 503
icyzkungz 0:f73c80b36c20 504 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 505 pc.printf("\nMaximum Hip Angle Range of Right Side = %f\n",temp);
icyzkungz 0:f73c80b36c20 506 bcom.setUpAngleRange(RIGHT_SIDE,temp,Rmin.Hip);
icyzkungz 0:f73c80b36c20 507 Rmax.Hip = temp;
icyzkungz 0:f73c80b36c20 508 } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_UP);
icyzkungz 0:f73c80b36c20 509 }
icyzkungz 0:f73c80b36c20 510 } while(temp!=9999);
icyzkungz 0:f73c80b36c20 511 }
icyzkungz 0:f73c80b36c20 512
icyzkungz 4:16cadf26dbab 513 else if(option == 28) { //Set Minumum Hip Angle Range
icyzkungz 0:f73c80b36c20 514 do {
icyzkungz 0:f73c80b36c20 515 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 516 pc.printf("1) Left Side\n");
icyzkungz 0:f73c80b36c20 517 pc.printf("2) Right Side\n");
icyzkungz 3:b7a8a60085c1 518 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 519
icyzkungz 0:f73c80b36c20 520 if(temp==1) { //Left
icyzkungz 0:f73c80b36c20 521 pc.printf("Input Minumum Hip Angle Range of Left Side\n");
icyzkungz 0:f73c80b36c20 522 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 523
icyzkungz 0:f73c80b36c20 524 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 525 pc.printf("\nMinumum Hip Angle Range of Left Side = %f\n",temp);
icyzkungz 0:f73c80b36c20 526 bcom.setLowAngleRange(LEFT_SIDE,Lmax.Hip,temp);
icyzkungz 0:f73c80b36c20 527 Lmin.Hip = temp;
icyzkungz 0:f73c80b36c20 528 } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_UP);
icyzkungz 0:f73c80b36c20 529
icyzkungz 0:f73c80b36c20 530 } else if(temp==2) { //Right
icyzkungz 0:f73c80b36c20 531 pc.printf("Input Minumum Hip Angle Range of Right Side\n");
icyzkungz 0:f73c80b36c20 532 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 533
icyzkungz 0:f73c80b36c20 534 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 535 pc.printf("\nMinumum Hip Angle Range of Right Side = %f\n",temp);
icyzkungz 0:f73c80b36c20 536 bcom.setLowAngleRange(RIGHT_SIDE,Rmax.Hip,temp);
icyzkungz 0:f73c80b36c20 537 Rmin.Hip = temp;
icyzkungz 0:f73c80b36c20 538 } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_UP);
icyzkungz 0:f73c80b36c20 539 }
icyzkungz 0:f73c80b36c20 540 } while(temp!=9999);
icyzkungz 0:f73c80b36c20 541 }
icyzkungz 0:f73c80b36c20 542
icyzkungz 0:f73c80b36c20 543
icyzkungz 4:16cadf26dbab 544 else if(option == 29) { //Set Maximum Knee Angle Range
icyzkungz 0:f73c80b36c20 545 do {
icyzkungz 0:f73c80b36c20 546 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 547 pc.printf("1) Left Side\n");
icyzkungz 0:f73c80b36c20 548 pc.printf("2) Right Side\n");
icyzkungz 3:b7a8a60085c1 549 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 550
icyzkungz 0:f73c80b36c20 551 if(temp==1) { //Left
icyzkungz 0:f73c80b36c20 552 pc.printf("Input Maximum Knee Angle Range of Left Side\n");
icyzkungz 0:f73c80b36c20 553 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 554
icyzkungz 0:f73c80b36c20 555 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 556 pc.printf("\nMaximum Knee Angle Range of Left Side = %f\n",temp);
icyzkungz 0:f73c80b36c20 557 bcom.setLowAngleRange(LEFT_SIDE,temp,Lmin.Knee);
icyzkungz 0:f73c80b36c20 558 Lmax.Knee = temp;
icyzkungz 0:f73c80b36c20 559 } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_LOW);
icyzkungz 0:f73c80b36c20 560
icyzkungz 0:f73c80b36c20 561 } else if(temp==2) { //Right
icyzkungz 0:f73c80b36c20 562 pc.printf("Input Maximum Knee Angle Range of Right Side\n");
icyzkungz 0:f73c80b36c20 563 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 564
icyzkungz 0:f73c80b36c20 565 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 566 pc.printf("\nMaximum Knee Angle Range of Right Side = %f\n",temp);
icyzkungz 0:f73c80b36c20 567 bcom.setLowAngleRange(RIGHT_SIDE,temp,Rmin.Knee);
icyzkungz 0:f73c80b36c20 568 Rmax.Knee = temp;
icyzkungz 0:f73c80b36c20 569 } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_LOW);
icyzkungz 0:f73c80b36c20 570 }
icyzkungz 0:f73c80b36c20 571 } while(temp!=9999);
icyzkungz 0:f73c80b36c20 572 }
icyzkungz 0:f73c80b36c20 573
icyzkungz 4:16cadf26dbab 574 else if(option == 30) { //Set Minumum Knee Angle Range
icyzkungz 0:f73c80b36c20 575 do {
icyzkungz 0:f73c80b36c20 576 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 0:f73c80b36c20 577 pc.printf("1) Left Side\n");
icyzkungz 0:f73c80b36c20 578 pc.printf("2) Right Side\n");
icyzkungz 3:b7a8a60085c1 579 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 580
icyzkungz 0:f73c80b36c20 581 if(temp==1) { //Left
icyzkungz 0:f73c80b36c20 582 pc.printf("Input Minumum Knee Angle Range of Left Side\n");
icyzkungz 0:f73c80b36c20 583 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 584
icyzkungz 0:f73c80b36c20 585 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 586 pc.printf("\nMinumum Knee Angle Range of Left Side = %f\n",temp);
icyzkungz 0:f73c80b36c20 587 bcom.setLowAngleRange(LEFT_SIDE,Lmax.Knee,temp);
icyzkungz 0:f73c80b36c20 588 Lmin.Knee = temp;
icyzkungz 0:f73c80b36c20 589 } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_LOW);
icyzkungz 0:f73c80b36c20 590
icyzkungz 0:f73c80b36c20 591 } else if(temp==2) { //Right
icyzkungz 0:f73c80b36c20 592 pc.printf("Input Minumum Knee Angle Range of Right Side\n");
icyzkungz 0:f73c80b36c20 593 pc.scanf("%f",&temp);
icyzkungz 0:f73c80b36c20 594
icyzkungz 0:f73c80b36c20 595 if(temp != 9999) {
icyzkungz 0:f73c80b36c20 596 pc.printf("\nMinumum Knee Angle Range of Right Side = %f\n",temp);
icyzkungz 0:f73c80b36c20 597 bcom.setLowAngleRange(RIGHT_SIDE,Rmax.Knee,temp);
icyzkungz 0:f73c80b36c20 598 Rmin.Knee = temp;
icyzkungz 0:f73c80b36c20 599 } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_LOW);
icyzkungz 0:f73c80b36c20 600 }
icyzkungz 0:f73c80b36c20 601 } while(temp!=9999);
icyzkungz 0:f73c80b36c20 602 }
icyzkungz 0:f73c80b36c20 603
icyzkungz 0:f73c80b36c20 604
icyzkungz 0:f73c80b36c20 605
icyzkungz 0:f73c80b36c20 606
icyzkungz 0:f73c80b36c20 607
icyzkungz 0:f73c80b36c20 608
icyzkungz 1:183e6088a1fa 609 else if(option == 40) {
icyzkungz 0:f73c80b36c20 610 pc.printf("Are You Sure ?\n");
icyzkungz 0:f73c80b36c20 611 pc.printf("\n1) Yes\n");
icyzkungz 0:f73c80b36c20 612 pc.printf("2) No\n");
icyzkungz 0:f73c80b36c20 613
icyzkungz 0:f73c80b36c20 614 pc.scanf("%d",&option);
icyzkungz 0:f73c80b36c20 615 if(option==1) {
icyzkungz 1:183e6088a1fa 616 option = 40;
icyzkungz 0:f73c80b36c20 617 pc.printf("Please Push Button Restart\n");
icyzkungz 0:f73c80b36c20 618 } else pc.printf("Return to Main Menu\n");
icyzkungz 0:f73c80b36c20 619
icyzkungz 0:f73c80b36c20 620 }
icyzkungz 0:f73c80b36c20 621
icyzkungz 0:f73c80b36c20 622
icyzkungz 2:d6be1c49d9d5 623 else { // if user has entered invalid choice
icyzkungz 1:183e6088a1fa 624 pc.printf("\nInvalid Option entered\n");
icyzkungz 0:f73c80b36c20 625 }
icyzkungz 2:d6be1c49d9d5 626 } while(option != 40);
icyzkungz 0:f73c80b36c20 627
icyzkungz 0:f73c80b36c20 628 }
icyzkungz 0:f73c80b36c20 629
icyzkungz 0:f73c80b36c20 630 int main()
icyzkungz 0:f73c80b36c20 631 {
icyzkungz 0:f73c80b36c20 632 pc.baud(115200);
icyzkungz 1:183e6088a1fa 633 pc.printf("Start\n");
icyzkungz 0:f73c80b36c20 634
icyzkungz 1:183e6088a1fa 635 if(!button) {
icyzkungz 1:183e6088a1fa 636 while(!button);
icyzkungz 0:f73c80b36c20 637 SwMode();
icyzkungz 1:183e6088a1fa 638 }
icyzkungz 1:183e6088a1fa 639
icyzkungz 0:f73c80b36c20 640 }
icyzkungz 0:f73c80b36c20 641
icyzkungz 0:f73c80b36c20 642 /*int main()
icyzkungz 0:f73c80b36c20 643 {
icyzkungz 4:16cadf26dbab 644 pc.baud(105200);
icyzkungz 0:f73c80b36c20 645 int num;
icyzkungz 0:f73c80b36c20 646 do {
icyzkungz 0:f73c80b36c20 647 //float num;
icyzkungz 0:f73c80b36c20 648 //Float number --> Working
icyzkungz 0:f73c80b36c20 649 pc.printf("Start\n");
icyzkungz 0:f73c80b36c20 650 pc.scanf("%d",&num);
icyzkungz 0:f73c80b36c20 651 pc.printf("temp : %d\n",num);
icyzkungz 0:f73c80b36c20 652 if( num == 15 )
icyzkungz 0:f73c80b36c20 653 pc.printf("Yes\n");
icyzkungz 0:f73c80b36c20 654 pc.printf("End\n");
icyzkungz 0:f73c80b36c20 655 } while(num!=20);
icyzkungz 0:f73c80b36c20 656 }*/