BE@R lab / Mbed 2 deprecated MPU9250AHRS-PGear_Stabilizer

Stabilizer

Dependencies:   BEAR_Protocol mbed Stabilizer iSerial

Fork of MPU9250AHRS by BE@R lab

main.cpp@19:5b8d16378d66, 2016-01-23 (annotated)

Committer:
soulx
Date:
Sat Jan 23 17:18:19 2016 +0000
Revision:
19:5b8d16378d66
Parent:
18:02559b51ec08
Child:
20:1d4db25afe2b
edit waring

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 18:02559b51ec08 1
icyzkungz 6:fa132fd505ce 2 #include "Stabilizer.h"
icyzkungz 10:f088c124d2be 3 #include "Kinematic.h"
onehorse 0:2e5e65a6fb30 4 #include "MPU9250.h"
onehorse 0:2e5e65a6fb30 5
soulx 18:02559b51ec08 6 #include "BEAR_Protocol.h"
soulx 18:02559b51ec08 7
soulx 18:02559b51ec08 8 #define LEFT_SIDE 0x01
soulx 18:02559b51ec08 9 #define RIGHT_SIDE 0x02
soulx 18:02559b51ec08 10
onehorse 0:2e5e65a6fb30 11 float sum = 0;
onehorse 0:2e5e65a6fb30 12 uint32_t sumCount = 0;
onehorse 0:2e5e65a6fb30 13 char buffer[14];
onehorse 0:2e5e65a6fb30 14
soulx 3:3e04c1c03cab 15 MPU9250 mpu9250;
soulx 14:8101a48eb972 16 Stabilizer Stabilize(5.0f,0.0f);
icyzkungz 17:51ac4252fa1a 17 //Kinematic L('Z',10,10,30,30),R('Z',10,10,30,30);
icyzkungz 17:51ac4252fa1a 18 Kinematic L(0,0),R(0,0);
icyzkungz 17:51ac4252fa1a 19 Bear_Communicate bcom(PA_15,PB_7,1000000);
soulx 3:3e04c1c03cab 20
soulx 3:3e04c1c03cab 21 Timer t;
onehorse 0:2e5e65a6fb30 22
soulx 3:3e04c1c03cab 23 Serial pc(USBTX, USBRX); // tx, rx
soulx 3:3e04c1c03cab 24
soulx 3:3e04c1c03cab 25 float xmax = -4914.0f;
soulx 3:3e04c1c03cab 26 float xmin = 4914.0f;
onehorse 0:2e5e65a6fb30 27
soulx 3:3e04c1c03cab 28 float ymax = -4914.0;
soulx 3:3e04c1c03cab 29 float ymin = 4914.0f;
soulx 3:3e04c1c03cab 30
soulx 3:3e04c1c03cab 31 float zmax = -4914.0;
soulx 3:3e04c1c03cab 32 float zmin = 4914.0f;
onehorse 0:2e5e65a6fb30 33
soulx 3:3e04c1c03cab 34 float Xsf,Ysf;
soulx 3:3e04c1c03cab 35 float Xoff,Yoff;
soulx 3:3e04c1c03cab 36
soulx 3:3e04c1c03cab 37
soulx 3:3e04c1c03cab 38 //InterruptIn event(PC_13);
soulx 3:3e04c1c03cab 39 DigitalIn enable(PC_13);
soulx 3:3e04c1c03cab 40
icyzkungz 16:b5b9827dd5dc 41 DigitalIn button(USER_BUTTON);
icyzkungz 16:b5b9827dd5dc 42
icyzkungz 16:b5b9827dd5dc 43 void SwMode()
icyzkungz 16:b5b9827dd5dc 44 {
icyzkungz 17:51ac4252fa1a 45 int option;
icyzkungz 17:51ac4252fa1a 46 float temp;
icyzkungz 16:b5b9827dd5dc 47 float LH,LK,RH,RK;
soulx 19:5b8d16378d66 48 //float LLink[2],RLink[2];
soulx 19:5b8d16378d66 49 float LLink0=0.0f,LLink1=0.0f,RLink0=0.0f,RLink1=0.0f;
icyzkungz 17:51ac4252fa1a 50 struct max_ang {
icyzkungz 17:51ac4252fa1a 51 float Hip;
icyzkungz 17:51ac4252fa1a 52 float Knee;
icyzkungz 17:51ac4252fa1a 53 };
icyzkungz 17:51ac4252fa1a 54 struct min_ang {
icyzkungz 17:51ac4252fa1a 55 float Hip;
icyzkungz 17:51ac4252fa1a 56 float Knee;
icyzkungz 17:51ac4252fa1a 57 };
icyzkungz 17:51ac4252fa1a 58 max_ang Lmax,Lmin;
icyzkungz 17:51ac4252fa1a 59 min_ang Rmax,Rmin;
icyzkungz 17:51ac4252fa1a 60 Lmax.Knee = Lmax.Hip = Lmin.Knee = Lmin.Hip = Rmax.Knee = Rmax.Hip = Rmin.Knee = Rmin.Hip = 0;
icyzkungz 17:51ac4252fa1a 61 bool a,b,c,d;
icyzkungz 17:51ac4252fa1a 62 a = b = c = d = false;
icyzkungz 16:b5b9827dd5dc 63 float temp_LH=0,temp_LK=0,temp_RH=0,temp_RK=0;
icyzkungz 17:51ac4252fa1a 64 do {
icyzkungz 16:b5b9827dd5dc 65 pc.printf("************Don't Forget to Save Data************\n");
icyzkungz 16:b5b9827dd5dc 66 pc.printf("*\t1) Motor Left Hip \t\t\t*\n");
icyzkungz 16:b5b9827dd5dc 67 pc.printf("*\t2) Motor Left Knee \t\t\t*\n");
icyzkungz 16:b5b9827dd5dc 68 pc.printf("*\t3) Motor Right Hip \t\t\t*\n");
icyzkungz 16:b5b9827dd5dc 69 pc.printf("*\t4) Motor Right Knee \t\t\t*\n");
icyzkungz 17:51ac4252fa1a 70
icyzkungz 16:b5b9827dd5dc 71 pc.printf("*\t5) Kp : Left Hip \t\t\t*\n");
icyzkungz 16:b5b9827dd5dc 72 pc.printf("*\t6) Ki : Left Hip \t\t\t*\n");
icyzkungz 16:b5b9827dd5dc 73 pc.printf("*\t7) Kd : Left Hip \t\t\t*\n");
icyzkungz 17:51ac4252fa1a 74
icyzkungz 17:51ac4252fa1a 75 pc.printf("*\t8) Kp : Left Knee \t\t\t*\n");
icyzkungz 17:51ac4252fa1a 76 pc.printf("*\t9) Ki : Left Knee \t\t\t*\n");
icyzkungz 17:51ac4252fa1a 77 pc.printf("*\t10) Kd : Left Knee \t\t\t*\n");
icyzkungz 17:51ac4252fa1a 78
icyzkungz 17:51ac4252fa1a 79 pc.printf("*\t11) Kp : Right Hip \t\t\t*\n");
icyzkungz 17:51ac4252fa1a 80 pc.printf("*\t12) Ki : Right Hip \t\t\t*\n");
icyzkungz 17:51ac4252fa1a 81 pc.printf("*\t13) Kd : Right Hip \t\t\t*\n");
icyzkungz 17:51ac4252fa1a 82
icyzkungz 17:51ac4252fa1a 83 pc.printf("*\t14) Kp : Right Knee \t\t\t*\n");
icyzkungz 17:51ac4252fa1a 84 pc.printf("*\t15) Ki : Right Knee \t\t\t*\n");
icyzkungz 17:51ac4252fa1a 85 pc.printf("*\t16) Kd : Right Knee \t\t\t*\n");
icyzkungz 17:51ac4252fa1a 86
icyzkungz 17:51ac4252fa1a 87 pc.printf("*\t17) Set Left Hip Margin\n");
icyzkungz 17:51ac4252fa1a 88 pc.printf("*\t18) Set Left Knee Margin\n");
icyzkungz 17:51ac4252fa1a 89 pc.printf("*\t19) Set Right Hip Margin\n");
icyzkungz 17:51ac4252fa1a 90 pc.printf("*\t20) Set Right Knee Margin\n");
icyzkungz 17:51ac4252fa1a 91
icyzkungz 17:51ac4252fa1a 92 pc.printf("*\t21) Set Lenght of Left Link Hip\n");
icyzkungz 17:51ac4252fa1a 93 pc.printf("*\t22) Set Lenght of Left Link Knee\n");
icyzkungz 17:51ac4252fa1a 94 pc.printf("*\t23) Set Lenght of Right Link Hip\n");
icyzkungz 17:51ac4252fa1a 95 pc.printf("*\t24) Set Lenght of Right Link Knee\n");
icyzkungz 17:51ac4252fa1a 96
icyzkungz 17:51ac4252fa1a 97 pc.printf("*\t25) Set Offset\n");
icyzkungz 17:51ac4252fa1a 98
icyzkungz 17:51ac4252fa1a 99 pc.printf("*\t26) Set Body Width\n");
icyzkungz 17:51ac4252fa1a 100
icyzkungz 17:51ac4252fa1a 101 pc.printf("*\t27) Set Maximum Hip Angle Range\n");
icyzkungz 17:51ac4252fa1a 102 pc.printf("*\t28) Set Minimum Hip Angle Range\n");
icyzkungz 17:51ac4252fa1a 103 pc.printf("*\t29) Set Maximum Knee Angle Range\n");
icyzkungz 17:51ac4252fa1a 104 pc.printf("*\t30) Set Minimum Knee Angle Range\n");
icyzkungz 17:51ac4252fa1a 105
icyzkungz 17:51ac4252fa1a 106 pc.printf("*\t40) Exit Program \t\t\t*\n");
icyzkungz 16:b5b9827dd5dc 107 pc.printf("************Don't Forget to Save Data************\n");
icyzkungz 16:b5b9827dd5dc 108 // Prompting user to enter an option according to menu
icyzkungz 16:b5b9827dd5dc 109 pc.printf("Please select an option : ");
icyzkungz 17:51ac4252fa1a 110
icyzkungz 16:b5b9827dd5dc 111 pc.scanf("%d",&option);
icyzkungz 17:51ac4252fa1a 112
icyzkungz 16:b5b9827dd5dc 113 pc.printf("\n");
icyzkungz 16:b5b9827dd5dc 114
icyzkungz 17:51ac4252fa1a 115 if(option == 1)// Left Hip
icyzkungz 16:b5b9827dd5dc 116 do {
icyzkungz 17:51ac4252fa1a 117 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 118 //Send Position to Motor
icyzkungz 17:51ac4252fa1a 119 pc.printf("Input Degree : \n");
icyzkungz 17:51ac4252fa1a 120 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 121 if(temp!=9999) {
icyzkungz 17:51ac4252fa1a 122 LH = temp;
icyzkungz 16:b5b9827dd5dc 123 pc.printf("Move Left Hip Motor to %f Degree\n",LH);
icyzkungz 16:b5b9827dd5dc 124
icyzkungz 17:51ac4252fa1a 125 bcom.setMotorPos(LEFT_SIDE,LH,temp_LK);
icyzkungz 16:b5b9827dd5dc 126 temp_LH = LH;
icyzkungz 16:b5b9827dd5dc 127
icyzkungz 16:b5b9827dd5dc 128 /********************Save Data*********************/
icyzkungz 17:51ac4252fa1a 129 } else bcom.saveDataToEEPROM(LEFT_SIDE,MOTOR_UPPER_ANG);
icyzkungz 16:b5b9827dd5dc 130
icyzkungz 17:51ac4252fa1a 131 } while(temp != 9999);
icyzkungz 17:51ac4252fa1a 132
icyzkungz 17:51ac4252fa1a 133 else if(option == 2) //Left Knee
icyzkungz 16:b5b9827dd5dc 134 do {
icyzkungz 17:51ac4252fa1a 135 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 16:b5b9827dd5dc 136
icyzkungz 17:51ac4252fa1a 137 //Send Position to Motor
icyzkungz 17:51ac4252fa1a 138 pc.printf("Input Degree : \n");
icyzkungz 17:51ac4252fa1a 139 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 140 if(temp!=9999) {
icyzkungz 17:51ac4252fa1a 141 LK = temp;
icyzkungz 16:b5b9827dd5dc 142 pc.printf("Move Left Knee Motor to %f Degree\n",LK);
icyzkungz 16:b5b9827dd5dc 143
icyzkungz 17:51ac4252fa1a 144 bcom.setMotorPos(LEFT_SIDE,temp_LH,LK);
icyzkungz 16:b5b9827dd5dc 145 temp_LK = LK;
icyzkungz 16:b5b9827dd5dc 146
icyzkungz 17:51ac4252fa1a 147
icyzkungz 16:b5b9827dd5dc 148 /********************Save Data*********************/
icyzkungz 17:51ac4252fa1a 149 } else bcom.saveDataToEEPROM(LEFT_SIDE,MOTOR_LOWER_ANG);
icyzkungz 16:b5b9827dd5dc 150
icyzkungz 17:51ac4252fa1a 151 } while(temp != 9999);
icyzkungz 17:51ac4252fa1a 152
icyzkungz 17:51ac4252fa1a 153
icyzkungz 17:51ac4252fa1a 154 else if(option == 3) { //Right Hip
icyzkungz 16:b5b9827dd5dc 155 do {
icyzkungz 17:51ac4252fa1a 156 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 16:b5b9827dd5dc 157
icyzkungz 17:51ac4252fa1a 158 //Send Position to Motor
icyzkungz 17:51ac4252fa1a 159 pc.printf("Input Degree : \n");
icyzkungz 17:51ac4252fa1a 160 pc.scanf("%f",&RH);
icyzkungz 17:51ac4252fa1a 161 if(temp!=9999) {
icyzkungz 17:51ac4252fa1a 162 RH = temp;
icyzkungz 16:b5b9827dd5dc 163 pc.printf("Move Right Hip Motor to %f Degree\n",RH);
icyzkungz 16:b5b9827dd5dc 164
icyzkungz 17:51ac4252fa1a 165 bcom.setMotorPos(RIGHT_SIDE,RH,temp_RK);
icyzkungz 16:b5b9827dd5dc 166 temp_RH = RH;
icyzkungz 16:b5b9827dd5dc 167
icyzkungz 17:51ac4252fa1a 168
icyzkungz 16:b5b9827dd5dc 169 /********************Save Data*********************/
icyzkungz 17:51ac4252fa1a 170 } else bcom.saveDataToEEPROM(RIGHT_SIDE,MOTOR_UPPER_ANG);
icyzkungz 17:51ac4252fa1a 171
icyzkungz 17:51ac4252fa1a 172 } while(temp != 9999);
icyzkungz 16:b5b9827dd5dc 173
icyzkungz 16:b5b9827dd5dc 174 } else if(option == 4) { //Right Knee
icyzkungz 16:b5b9827dd5dc 175 do {
icyzkungz 17:51ac4252fa1a 176 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 16:b5b9827dd5dc 177
icyzkungz 17:51ac4252fa1a 178 //Send Position to Motor
icyzkungz 17:51ac4252fa1a 179 pc.printf("Input Degree : \n");
icyzkungz 17:51ac4252fa1a 180 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 181
icyzkungz 17:51ac4252fa1a 182 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 183 RK = temp;
icyzkungz 16:b5b9827dd5dc 184 pc.printf("Move Right Knee Motor to %f Degree\n",RK);
icyzkungz 16:b5b9827dd5dc 185
icyzkungz 17:51ac4252fa1a 186 bcom.setMotorPos(RIGHT_SIDE,temp_RH,RK);
icyzkungz 16:b5b9827dd5dc 187 temp_RK = RK;
icyzkungz 16:b5b9827dd5dc 188
icyzkungz 17:51ac4252fa1a 189
icyzkungz 16:b5b9827dd5dc 190 /********************Save Data*********************/
icyzkungz 17:51ac4252fa1a 191 } else bcom.saveDataToEEPROM(RIGHT_SIDE,MOTOR_LOWER_ANG);
icyzkungz 17:51ac4252fa1a 192
icyzkungz 17:51ac4252fa1a 193 } while(temp != 9999);
icyzkungz 16:b5b9827dd5dc 194
icyzkungz 16:b5b9827dd5dc 195 } else if(option == 5) { //Left Hip
icyzkungz 16:b5b9827dd5dc 196 do {
icyzkungz 17:51ac4252fa1a 197 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 198 pc.printf("\nInput Kp of Left Hip\n");
icyzkungz 17:51ac4252fa1a 199 pc.scanf("%f",&temp);
icyzkungz 16:b5b9827dd5dc 200
icyzkungz 17:51ac4252fa1a 201 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 202 pc.printf("\nChange Kp of Left Hip to %f",temp);
icyzkungz 16:b5b9827dd5dc 203
icyzkungz 17:51ac4252fa1a 204 bcom.setUpMotorKp(LEFT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 205 } else bcom.saveDataToEEPROM(LEFT_SIDE,KP_UPPER_MOTOR);
icyzkungz 17:51ac4252fa1a 206
icyzkungz 17:51ac4252fa1a 207 } while(temp != 9999);
icyzkungz 16:b5b9827dd5dc 208
icyzkungz 16:b5b9827dd5dc 209 } else if(option == 6) { //Left Hip
icyzkungz 16:b5b9827dd5dc 210 do {
icyzkungz 17:51ac4252fa1a 211 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 212 pc.printf("\nInput Ki of Left Hip\n");
icyzkungz 17:51ac4252fa1a 213 pc.scanf("%f",&temp);
icyzkungz 16:b5b9827dd5dc 214
icyzkungz 17:51ac4252fa1a 215 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 216 pc.printf("\nChange Ki of Left Hip to %f",temp);
icyzkungz 16:b5b9827dd5dc 217
icyzkungz 17:51ac4252fa1a 218 bcom.setUpMotorKi(LEFT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 219 } else bcom.saveDataToEEPROM(LEFT_SIDE,KI_UPPER_MOTOR);
icyzkungz 17:51ac4252fa1a 220
icyzkungz 17:51ac4252fa1a 221 } while(temp != 9999);
icyzkungz 16:b5b9827dd5dc 222
icyzkungz 16:b5b9827dd5dc 223 } else if(option == 7) { //Left Hip
icyzkungz 16:b5b9827dd5dc 224 do {
icyzkungz 17:51ac4252fa1a 225 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 226 pc.printf("\nInput Kd of Left Hip\n");
icyzkungz 17:51ac4252fa1a 227 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 228
icyzkungz 17:51ac4252fa1a 229 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 230 pc.printf("\nChange Kd of Left Hip to %f",temp);
icyzkungz 17:51ac4252fa1a 231
icyzkungz 17:51ac4252fa1a 232 bcom.setUpMotorKd(LEFT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 233 } else bcom.saveDataToEEPROM(LEFT_SIDE,KD_UPPER_MOTOR);
icyzkungz 17:51ac4252fa1a 234
icyzkungz 17:51ac4252fa1a 235 } while(temp != 9999);
icyzkungz 16:b5b9827dd5dc 236
icyzkungz 17:51ac4252fa1a 237 } else if(option == 8) { //Left Knee
icyzkungz 17:51ac4252fa1a 238 do {
icyzkungz 17:51ac4252fa1a 239 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 240 pc.printf("\nInput Kp of Left Knee\n");
icyzkungz 17:51ac4252fa1a 241 pc.scanf("%f",&temp);
icyzkungz 16:b5b9827dd5dc 242
icyzkungz 17:51ac4252fa1a 243 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 244 pc.printf("\nChange Kp of Left Knee to %f",temp);
icyzkungz 16:b5b9827dd5dc 245
icyzkungz 17:51ac4252fa1a 246 bcom.setLowMotorKp(LEFT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 247 } else bcom.saveDataToEEPROM(LEFT_SIDE,KP_LOWER_MOTOR);
icyzkungz 17:51ac4252fa1a 248
icyzkungz 17:51ac4252fa1a 249 } while(temp != 9999);
icyzkungz 11:fceaaa7e120a 250
icyzkungz 16:b5b9827dd5dc 251 } else if(option == 9) { //Left Knee
icyzkungz 16:b5b9827dd5dc 252 do {
icyzkungz 17:51ac4252fa1a 253 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 254 pc.printf("\nInput Ki of Left Knee\n");
icyzkungz 17:51ac4252fa1a 255 pc.scanf("%f",&temp);
icyzkungz 16:b5b9827dd5dc 256
icyzkungz 17:51ac4252fa1a 257 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 258 pc.printf("\nChange Ki of Left Knee to %f",temp);
icyzkungz 16:b5b9827dd5dc 259
icyzkungz 17:51ac4252fa1a 260 bcom.setLowMotorKi(LEFT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 261 } else bcom.saveDataToEEPROM(LEFT_SIDE,KI_LOWER_MOTOR);
icyzkungz 17:51ac4252fa1a 262
icyzkungz 17:51ac4252fa1a 263 } while(temp != 9999);
icyzkungz 16:b5b9827dd5dc 264
icyzkungz 16:b5b9827dd5dc 265 } else if(option == 10) { //Left Knee
icyzkungz 16:b5b9827dd5dc 266 do {
icyzkungz 17:51ac4252fa1a 267 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 268 pc.printf("\nInput Kd of Left Knee\n");
icyzkungz 17:51ac4252fa1a 269 pc.scanf("%f",&temp);
icyzkungz 16:b5b9827dd5dc 270
icyzkungz 17:51ac4252fa1a 271 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 272 pc.printf("\nChange Kd of Left Knee to %f",temp);
icyzkungz 16:b5b9827dd5dc 273
icyzkungz 17:51ac4252fa1a 274 bcom.setLowMotorKd(LEFT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 275 } else bcom.saveDataToEEPROM(LEFT_SIDE,KD_LOWER_MOTOR);
icyzkungz 16:b5b9827dd5dc 276
icyzkungz 17:51ac4252fa1a 277 } while(temp != 9999);
icyzkungz 16:b5b9827dd5dc 278
icyzkungz 16:b5b9827dd5dc 279
icyzkungz 17:51ac4252fa1a 280 } else if(option == 11) { //Right Hip
icyzkungz 17:51ac4252fa1a 281 do {
icyzkungz 17:51ac4252fa1a 282 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 283 pc.printf("\nInput Kp of Right Hip\n");
icyzkungz 17:51ac4252fa1a 284 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 285
icyzkungz 17:51ac4252fa1a 286 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 287 pc.printf("\nChange Kp of Right Hip to %f",temp);
icyzkungz 17:51ac4252fa1a 288
icyzkungz 17:51ac4252fa1a 289 bcom.setUpMotorKp(RIGHT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 290 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KP_UPPER_MOTOR);
icyzkungz 17:51ac4252fa1a 291
icyzkungz 17:51ac4252fa1a 292 } while(temp != 9999);
icyzkungz 16:b5b9827dd5dc 293
icyzkungz 17:51ac4252fa1a 294 } else if(option == 12) { //Right Hip
icyzkungz 17:51ac4252fa1a 295 do {
icyzkungz 17:51ac4252fa1a 296 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 297 pc.printf("\nInput Ki of Right Hip\n");
icyzkungz 17:51ac4252fa1a 298 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 299
icyzkungz 17:51ac4252fa1a 300 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 301 pc.printf("\nChange Ki of Right Hip to %f",temp);
icyzkungz 17:51ac4252fa1a 302
icyzkungz 17:51ac4252fa1a 303 bcom.setUpMotorKi(RIGHT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 304 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KI_UPPER_MOTOR);
icyzkungz 17:51ac4252fa1a 305
icyzkungz 17:51ac4252fa1a 306 } while(temp != 9999);
icyzkungz 16:b5b9827dd5dc 307
icyzkungz 16:b5b9827dd5dc 308 } else if(option == 13) { //Right Hip
icyzkungz 16:b5b9827dd5dc 309 do {
icyzkungz 17:51ac4252fa1a 310 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 311 pc.printf("\nInput Kd of Right Hip\n");
icyzkungz 17:51ac4252fa1a 312 pc.scanf("%f",&temp);
icyzkungz 16:b5b9827dd5dc 313
icyzkungz 17:51ac4252fa1a 314 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 315 pc.printf("\nChange Kd of Right Hip to %f",temp);
icyzkungz 16:b5b9827dd5dc 316
icyzkungz 17:51ac4252fa1a 317 bcom.setUpMotorKd(RIGHT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 318 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KD_UPPER_MOTOR);
icyzkungz 16:b5b9827dd5dc 319
icyzkungz 17:51ac4252fa1a 320 } while(temp != 9999);
icyzkungz 16:b5b9827dd5dc 321
icyzkungz 16:b5b9827dd5dc 322
icyzkungz 17:51ac4252fa1a 323 } else if(option == 14) { //Right Knee
icyzkungz 17:51ac4252fa1a 324 do {
icyzkungz 17:51ac4252fa1a 325 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 326 pc.printf("\nInput Kp of Right Knee\n");
icyzkungz 17:51ac4252fa1a 327 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 328
icyzkungz 17:51ac4252fa1a 329 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 330 pc.printf("\nChange Kp of Right Knee to %f",temp);
icyzkungz 17:51ac4252fa1a 331
icyzkungz 17:51ac4252fa1a 332 bcom.setLowMotorKp(RIGHT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 333 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KP_LOWER_MOTOR);
icyzkungz 17:51ac4252fa1a 334
icyzkungz 17:51ac4252fa1a 335 } while(temp != 9999);
icyzkungz 17:51ac4252fa1a 336
icyzkungz 17:51ac4252fa1a 337 } else if(option == 15) { //Right Knee
icyzkungz 17:51ac4252fa1a 338 do {
icyzkungz 17:51ac4252fa1a 339 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 340 pc.printf("\nInput Ki of Right Knee\n");
icyzkungz 17:51ac4252fa1a 341 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 342
icyzkungz 17:51ac4252fa1a 343 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 344 pc.printf("\nChange Ki of Right Knee to %f Degree\n",temp);
icyzkungz 17:51ac4252fa1a 345
icyzkungz 17:51ac4252fa1a 346 bcom.setLowMotorKi(RIGHT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 347 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KI_LOWER_MOTOR);
icyzkungz 17:51ac4252fa1a 348
icyzkungz 17:51ac4252fa1a 349 } while(temp != 9999);
icyzkungz 17:51ac4252fa1a 350
icyzkungz 17:51ac4252fa1a 351 } else if(option == 16) { //Right Knee
icyzkungz 17:51ac4252fa1a 352 do {
icyzkungz 17:51ac4252fa1a 353 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 354 pc.printf("\nInput Kd of Right Knee\n");
icyzkungz 17:51ac4252fa1a 355 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 356
icyzkungz 17:51ac4252fa1a 357 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 358 pc.printf("\nChange Kd of Right Knee to %f",temp);
icyzkungz 17:51ac4252fa1a 359
icyzkungz 17:51ac4252fa1a 360 bcom.setLowMotorKd(RIGHT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 361 } else bcom.saveDataToEEPROM(RIGHT_SIDE,KD_LOWER_MOTOR);
icyzkungz 17:51ac4252fa1a 362
icyzkungz 17:51ac4252fa1a 363 } while(temp != 9999);
icyzkungz 16:b5b9827dd5dc 364
icyzkungz 16:b5b9827dd5dc 365
icyzkungz 17:51ac4252fa1a 366 } else if(option == 17) { //Left Hip Margin
icyzkungz 17:51ac4252fa1a 367 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 368 pc.printf("Input Margin\n");
icyzkungz 17:51ac4252fa1a 369 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 370
icyzkungz 17:51ac4252fa1a 371 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 372 pc.printf("\nChange Left Hip Margin to %f\n",temp);
icyzkungz 17:51ac4252fa1a 373 bcom.setUpMargin(LEFT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 374 } else bcom.saveDataToEEPROM(LEFT_SIDE,UP_MARGIN);
icyzkungz 17:51ac4252fa1a 375 }
icyzkungz 17:51ac4252fa1a 376
icyzkungz 17:51ac4252fa1a 377 else if(option == 18) { //Left Knee Margin
icyzkungz 17:51ac4252fa1a 378 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 379 pc.printf("Input Margin\n");
icyzkungz 17:51ac4252fa1a 380 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 381
icyzkungz 17:51ac4252fa1a 382 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 383 pc.printf("\nChange Left Knee Margin to %f\n",temp);
icyzkungz 17:51ac4252fa1a 384 bcom.setLowMargin(LEFT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 385 } else bcom.saveDataToEEPROM(LEFT_SIDE,LOW_MARGIN);
icyzkungz 17:51ac4252fa1a 386 }
icyzkungz 16:b5b9827dd5dc 387
icyzkungz 17:51ac4252fa1a 388 else if(option == 19) { //Right Hip Margin
icyzkungz 17:51ac4252fa1a 389 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 390 pc.printf("Input Margin\n");
icyzkungz 17:51ac4252fa1a 391 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 392
icyzkungz 17:51ac4252fa1a 393 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 394 pc.printf("\nChange Right Hip Margin to %f\n",temp);
icyzkungz 17:51ac4252fa1a 395 bcom.setUpMargin(RIGHT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 396 } else bcom.saveDataToEEPROM(RIGHT_SIDE,UP_MARGIN);
icyzkungz 17:51ac4252fa1a 397 }
icyzkungz 16:b5b9827dd5dc 398
icyzkungz 17:51ac4252fa1a 399 else if(option == 20) { //Right Knee Margin
icyzkungz 17:51ac4252fa1a 400 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 401 pc.printf("Input Margin\n");
icyzkungz 17:51ac4252fa1a 402 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 403
icyzkungz 17:51ac4252fa1a 404 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 405 pc.printf("\nChange Right Knee Margin to %f\n",temp);
icyzkungz 17:51ac4252fa1a 406 bcom.setLowMargin(RIGHT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 407 } else bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_MARGIN);
icyzkungz 17:51ac4252fa1a 408 }
icyzkungz 16:b5b9827dd5dc 409
icyzkungz 17:51ac4252fa1a 410 else if(option == 21) { //Lenght of Left Link Hip
icyzkungz 17:51ac4252fa1a 411 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 412 pc.printf("Input Lenght of Left Link Hip\n");
icyzkungz 17:51ac4252fa1a 413 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 414
icyzkungz 17:51ac4252fa1a 415 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 416 pc.printf("\nLenght of Left Link Hip = %f\n",temp);
icyzkungz 17:51ac4252fa1a 417 LLink0 = temp;
icyzkungz 17:51ac4252fa1a 418 a = true;
icyzkungz 17:51ac4252fa1a 419 bcom.setUpLinkLength(LEFT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 420 } else bcom.saveDataToEEPROM(LEFT_SIDE,UP_LINK_LENGTH);
icyzkungz 17:51ac4252fa1a 421 }
icyzkungz 17:51ac4252fa1a 422
icyzkungz 17:51ac4252fa1a 423 else if(option == 22) { //Lenght of Left Link Knee
icyzkungz 17:51ac4252fa1a 424 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 425 pc.printf("Input Lenght of Left Link Knee\n");
icyzkungz 17:51ac4252fa1a 426 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 427
icyzkungz 17:51ac4252fa1a 428 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 429 pc.printf("\nLenght of Left Link Knee = %f\n",temp);
icyzkungz 17:51ac4252fa1a 430 LLink1 = temp;
icyzkungz 17:51ac4252fa1a 431 b = true;
icyzkungz 17:51ac4252fa1a 432 bcom.setLowLinkLength(LEFT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 433 } else bcom.saveDataToEEPROM(LEFT_SIDE,LOW_LINK_LENGTH);
icyzkungz 17:51ac4252fa1a 434 }
icyzkungz 16:b5b9827dd5dc 435
icyzkungz 17:51ac4252fa1a 436 else if(option == 23) { //Lenght of Right Link Hip
icyzkungz 17:51ac4252fa1a 437 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 438 pc.printf("Input Lenght of Right Link Hip\n");
icyzkungz 17:51ac4252fa1a 439 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 440
icyzkungz 17:51ac4252fa1a 441 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 442 pc.printf("\nLenght of Right Link Hip = %f\n",temp);
icyzkungz 17:51ac4252fa1a 443 RLink0 = temp;
icyzkungz 17:51ac4252fa1a 444 c = true;
icyzkungz 17:51ac4252fa1a 445 bcom.setUpLinkLength(RIGHT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 446 } else bcom.saveDataToEEPROM(RIGHT_SIDE,UP_LINK_LENGTH);
icyzkungz 17:51ac4252fa1a 447 }
icyzkungz 16:b5b9827dd5dc 448
icyzkungz 17:51ac4252fa1a 449 else if(option == 24) { //Lenght of Right Link Knee
icyzkungz 17:51ac4252fa1a 450 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 451 pc.printf("Input Lenght of Right Link Knee\n");
icyzkungz 17:51ac4252fa1a 452 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 453
icyzkungz 17:51ac4252fa1a 454 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 455 pc.printf("\nLenght of Right Link Knee = %f\n",temp);
icyzkungz 17:51ac4252fa1a 456 RLink1 = temp;
icyzkungz 17:51ac4252fa1a 457 d = true;
icyzkungz 17:51ac4252fa1a 458 bcom.setLowLinkLength(RIGHT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 459 } else bcom.saveDataToEEPROM(RIGHT_SIDE,LOW_LINK_LENGTH);
icyzkungz 17:51ac4252fa1a 460 }
icyzkungz 16:b5b9827dd5dc 461
icyzkungz 17:51ac4252fa1a 462 else if(option == 25) { //Offset
icyzkungz 17:51ac4252fa1a 463 if(a == true && b == true && c == true && d == true) {
icyzkungz 17:51ac4252fa1a 464 float LHipAngle,LKneeAngle;
icyzkungz 17:51ac4252fa1a 465 float RHipAngle,RKneeAngle;
icyzkungz 17:51ac4252fa1a 466 bcom.getMotorPos(LEFT_SIDE,&LHipAngle,&LKneeAngle);
icyzkungz 17:51ac4252fa1a 467 wait_ms(90);
icyzkungz 17:51ac4252fa1a 468 bcom.getMotorPos(RIGHT_SIDE,&RHipAngle,&RKneeAngle);
icyzkungz 17:51ac4252fa1a 469 wait_ms(90);
icyzkungz 17:51ac4252fa1a 470
icyzkungz 17:51ac4252fa1a 471 L.set_Link_Hip(LLink0);
icyzkungz 17:51ac4252fa1a 472 L.set_Link_Knee(LLink1);
icyzkungz 17:51ac4252fa1a 473 L.set_Zeta_Hip(LHipAngle);
icyzkungz 17:51ac4252fa1a 474 L.set_Zeta_Knee(LKneeAngle);
icyzkungz 17:51ac4252fa1a 475
icyzkungz 17:51ac4252fa1a 476 R.set_Link_Hip(RLink0);
icyzkungz 17:51ac4252fa1a 477 R.set_Link_Knee(RLink1);
icyzkungz 17:51ac4252fa1a 478 R.set_Zeta_Hip(RHipAngle);
icyzkungz 17:51ac4252fa1a 479 R.set_Zeta_Knee(RKneeAngle);
icyzkungz 17:51ac4252fa1a 480
icyzkungz 17:51ac4252fa1a 481 L.ForwardKinematicCalculation();
soulx 18:02559b51ec08 482 R.ForwardKinematicCalculation();
icyzkungz 16:b5b9827dd5dc 483
icyzkungz 17:51ac4252fa1a 484 float offset_Y,offset_Z;
icyzkungz 17:51ac4252fa1a 485 float y1,y2,z1,z2;
icyzkungz 17:51ac4252fa1a 486 y1 = L.get_Position_Y();
icyzkungz 17:51ac4252fa1a 487 y2 = R.get_Position_Y();
icyzkungz 17:51ac4252fa1a 488 z1 = L.get_Position_Z();
icyzkungz 17:51ac4252fa1a 489 z2 = R.get_Position_Z();
icyzkungz 17:51ac4252fa1a 490 offset_Y = y1-y2;
icyzkungz 17:51ac4252fa1a 491 offset_Z = z1-z2;
icyzkungz 17:51ac4252fa1a 492
icyzkungz 17:51ac4252fa1a 493 bcom.setOffset(LEFT_SIDE,offset_Y,offset_Z);
icyzkungz 17:51ac4252fa1a 494
icyzkungz 17:51ac4252fa1a 495 bcom.saveDataToEEPROM(LEFT_SIDE,OFFSET);
icyzkungz 16:b5b9827dd5dc 496
icyzkungz 17:51ac4252fa1a 497 } else {
icyzkungz 17:51ac4252fa1a 498 pc.printf("\nYou have to do choice 21-23 first\n\n");
icyzkungz 17:51ac4252fa1a 499 wait(2);
icyzkungz 17:51ac4252fa1a 500 }
icyzkungz 17:51ac4252fa1a 501 }
icyzkungz 17:51ac4252fa1a 502
icyzkungz 17:51ac4252fa1a 503 else if(option == 26) { //setBodyWidth
icyzkungz 17:51ac4252fa1a 504 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 505 pc.printf("Input Body Width\n");
icyzkungz 17:51ac4252fa1a 506 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 507 pc.printf("\nBody Lenght = %f\n",temp);
icyzkungz 17:51ac4252fa1a 508 bcom.setBodyWidth(LEFT_SIDE,temp);
icyzkungz 17:51ac4252fa1a 509 bcom.saveDataToEEPROM(LEFT_SIDE,BODY_WIDTH);
icyzkungz 17:51ac4252fa1a 510 }
icyzkungz 16:b5b9827dd5dc 511
icyzkungz 17:51ac4252fa1a 512 else if(option == 27) { //Set Maximum Hip Angle Range
icyzkungz 17:51ac4252fa1a 513 do {
icyzkungz 17:51ac4252fa1a 514 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 515 pc.printf("1) Left Side\n");
icyzkungz 17:51ac4252fa1a 516 pc.printf("2) Right Side\n");
icyzkungz 17:51ac4252fa1a 517 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 518
icyzkungz 17:51ac4252fa1a 519 if(temp==1) { //Left
icyzkungz 17:51ac4252fa1a 520 pc.printf("Input Maximum Hip Angle Range of Left Side\n");
icyzkungz 17:51ac4252fa1a 521 pc.scanf("%f",&temp);
icyzkungz 16:b5b9827dd5dc 522
icyzkungz 17:51ac4252fa1a 523 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 524 pc.printf("\nMaximum Hip Angle Range of Left Side = %f\n",temp);
icyzkungz 17:51ac4252fa1a 525 bcom.setUpAngleRange(LEFT_SIDE,temp,Lmin.Hip);
icyzkungz 17:51ac4252fa1a 526 Lmax.Hip = temp;
icyzkungz 17:51ac4252fa1a 527 } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_UP);
icyzkungz 17:51ac4252fa1a 528
icyzkungz 17:51ac4252fa1a 529 } else if(temp==2) { //Right
icyzkungz 17:51ac4252fa1a 530 pc.printf("Input Maximum Hip Angle Range of Right Side\n");
icyzkungz 17:51ac4252fa1a 531 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 532
icyzkungz 17:51ac4252fa1a 533 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 534 pc.printf("\nMaximum Hip Angle Range of Right Side = %f\n",temp);
icyzkungz 17:51ac4252fa1a 535 bcom.setUpAngleRange(RIGHT_SIDE,temp,Rmin.Hip);
icyzkungz 17:51ac4252fa1a 536 Rmax.Hip = temp;
icyzkungz 17:51ac4252fa1a 537 } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_UP);
icyzkungz 17:51ac4252fa1a 538 }
icyzkungz 17:51ac4252fa1a 539 } while(temp!=9999);
icyzkungz 17:51ac4252fa1a 540 }
icyzkungz 16:b5b9827dd5dc 541
icyzkungz 17:51ac4252fa1a 542 else if(option == 28) { //Set Minumum Hip Angle Range
icyzkungz 17:51ac4252fa1a 543 do {
icyzkungz 17:51ac4252fa1a 544 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 545 pc.printf("1) Left Side\n");
icyzkungz 17:51ac4252fa1a 546 pc.printf("2) Right Side\n");
icyzkungz 17:51ac4252fa1a 547 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 548
icyzkungz 17:51ac4252fa1a 549 if(temp==1) { //Left
icyzkungz 17:51ac4252fa1a 550 pc.printf("Input Minumum Hip Angle Range of Left Side\n");
icyzkungz 17:51ac4252fa1a 551 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 552
icyzkungz 17:51ac4252fa1a 553 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 554 pc.printf("\nMinumum Hip Angle Range of Left Side = %f\n",temp);
icyzkungz 17:51ac4252fa1a 555 bcom.setLowAngleRange(LEFT_SIDE,Lmax.Hip,temp);
icyzkungz 17:51ac4252fa1a 556 Lmin.Hip = temp;
icyzkungz 17:51ac4252fa1a 557 } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_UP);
icyzkungz 17:51ac4252fa1a 558
icyzkungz 17:51ac4252fa1a 559 } else if(temp==2) { //Right
icyzkungz 17:51ac4252fa1a 560 pc.printf("Input Minumum Hip Angle Range of Right Side\n");
icyzkungz 17:51ac4252fa1a 561 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 562
icyzkungz 17:51ac4252fa1a 563 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 564 pc.printf("\nMinumum Hip Angle Range of Right Side = %f\n",temp);
icyzkungz 17:51ac4252fa1a 565 bcom.setLowAngleRange(RIGHT_SIDE,Rmax.Hip,temp);
icyzkungz 17:51ac4252fa1a 566 Rmin.Hip = temp;
icyzkungz 17:51ac4252fa1a 567 } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_UP);
icyzkungz 16:b5b9827dd5dc 568 }
icyzkungz 17:51ac4252fa1a 569 } while(temp!=9999);
icyzkungz 17:51ac4252fa1a 570 }
icyzkungz 16:b5b9827dd5dc 571
icyzkungz 16:b5b9827dd5dc 572
icyzkungz 17:51ac4252fa1a 573 else if(option == 29) { //Set Maximum Knee Angle Range
icyzkungz 17:51ac4252fa1a 574 do {
icyzkungz 17:51ac4252fa1a 575 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 576 pc.printf("1) Left Side\n");
icyzkungz 17:51ac4252fa1a 577 pc.printf("2) Right Side\n");
icyzkungz 17:51ac4252fa1a 578 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 579
icyzkungz 17:51ac4252fa1a 580 if(temp==1) { //Left
icyzkungz 17:51ac4252fa1a 581 pc.printf("Input Maximum Knee Angle Range of Left Side\n");
icyzkungz 17:51ac4252fa1a 582 pc.scanf("%f",&temp);
icyzkungz 16:b5b9827dd5dc 583
icyzkungz 17:51ac4252fa1a 584 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 585 pc.printf("\nMaximum Knee Angle Range of Left Side = %f\n",temp);
icyzkungz 17:51ac4252fa1a 586 bcom.setLowAngleRange(LEFT_SIDE,temp,Lmin.Knee);
icyzkungz 17:51ac4252fa1a 587 Lmax.Knee = temp;
icyzkungz 17:51ac4252fa1a 588 } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_LOW);
icyzkungz 17:51ac4252fa1a 589
icyzkungz 17:51ac4252fa1a 590 } else if(temp==2) { //Right
icyzkungz 17:51ac4252fa1a 591 pc.printf("Input Maximum Knee Angle Range of Right Side\n");
icyzkungz 17:51ac4252fa1a 592 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 593
icyzkungz 17:51ac4252fa1a 594 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 595 pc.printf("\nMaximum Knee Angle Range of Right Side = %f\n",temp);
icyzkungz 17:51ac4252fa1a 596 bcom.setLowAngleRange(RIGHT_SIDE,temp,Rmin.Knee);
icyzkungz 17:51ac4252fa1a 597 Rmax.Knee = temp;
icyzkungz 17:51ac4252fa1a 598 } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_LOW);
icyzkungz 17:51ac4252fa1a 599 }
icyzkungz 17:51ac4252fa1a 600 } while(temp!=9999);
icyzkungz 17:51ac4252fa1a 601 }
icyzkungz 16:b5b9827dd5dc 602
icyzkungz 17:51ac4252fa1a 603 else if(option == 30) { //Set Minumum Knee Angle Range
icyzkungz 17:51ac4252fa1a 604 do {
icyzkungz 17:51ac4252fa1a 605 pc.printf("\nType 9999 to Exit to Main Menu\n");
icyzkungz 17:51ac4252fa1a 606 pc.printf("1) Left Side\n");
icyzkungz 17:51ac4252fa1a 607 pc.printf("2) Right Side\n");
icyzkungz 17:51ac4252fa1a 608 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 609
icyzkungz 17:51ac4252fa1a 610 if(temp==1) { //Left
icyzkungz 17:51ac4252fa1a 611 pc.printf("Input Minumum Knee Angle Range of Left Side\n");
icyzkungz 17:51ac4252fa1a 612 pc.scanf("%f",&temp);
icyzkungz 17:51ac4252fa1a 613
icyzkungz 17:51ac4252fa1a 614 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 615 pc.printf("\nMinumum Knee Angle Range of Left Side = %f\n",temp);
icyzkungz 17:51ac4252fa1a 616 bcom.setLowAngleRange(LEFT_SIDE,Lmax.Knee,temp);
icyzkungz 17:51ac4252fa1a 617 Lmin.Knee = temp;
icyzkungz 17:51ac4252fa1a 618 } else bcom.saveDataToEEPROM(LEFT_SIDE,ANGLE_RANGE_LOW);
icyzkungz 16:b5b9827dd5dc 619
icyzkungz 17:51ac4252fa1a 620 } else if(temp==2) { //Right
icyzkungz 17:51ac4252fa1a 621 pc.printf("Input Minumum Knee Angle Range of Right Side\n");
icyzkungz 17:51ac4252fa1a 622 pc.scanf("%f",&temp);
icyzkungz 16:b5b9827dd5dc 623
icyzkungz 17:51ac4252fa1a 624 if(temp != 9999) {
icyzkungz 17:51ac4252fa1a 625 pc.printf("\nMinumum Knee Angle Range of Right Side = %f\n",temp);
icyzkungz 17:51ac4252fa1a 626 bcom.setLowAngleRange(RIGHT_SIDE,Rmax.Knee,temp);
icyzkungz 17:51ac4252fa1a 627 Rmin.Knee = temp;
icyzkungz 17:51ac4252fa1a 628 } else bcom.saveDataToEEPROM(RIGHT_SIDE,ANGLE_RANGE_LOW);
icyzkungz 17:51ac4252fa1a 629 }
icyzkungz 17:51ac4252fa1a 630 } while(temp!=9999);
icyzkungz 17:51ac4252fa1a 631 }
icyzkungz 17:51ac4252fa1a 632
icyzkungz 17:51ac4252fa1a 633
icyzkungz 17:51ac4252fa1a 634
icyzkungz 17:51ac4252fa1a 635
icyzkungz 17:51ac4252fa1a 636
icyzkungz 17:51ac4252fa1a 637
icyzkungz 17:51ac4252fa1a 638 else if(option == 40) {
icyzkungz 16:b5b9827dd5dc 639 pc.printf("Are You Sure ?\n");
icyzkungz 16:b5b9827dd5dc 640 pc.printf("\n1) Yes\n");
icyzkungz 16:b5b9827dd5dc 641 pc.printf("2) No\n");
icyzkungz 16:b5b9827dd5dc 642
icyzkungz 16:b5b9827dd5dc 643 pc.scanf("%d",&option);
icyzkungz 16:b5b9827dd5dc 644 if(option==1) {
icyzkungz 17:51ac4252fa1a 645 option = 40;
icyzkungz 16:b5b9827dd5dc 646 pc.printf("Please Push Button Restart\n");
icyzkungz 16:b5b9827dd5dc 647 } else pc.printf("Return to Main Menu\n");
icyzkungz 16:b5b9827dd5dc 648
icyzkungz 16:b5b9827dd5dc 649 }
icyzkungz 16:b5b9827dd5dc 650
icyzkungz 16:b5b9827dd5dc 651
icyzkungz 17:51ac4252fa1a 652 else { // if user has entered invalid choice
icyzkungz 17:51ac4252fa1a 653 pc.printf("\nInvalid Option entered\n");
icyzkungz 16:b5b9827dd5dc 654 }
icyzkungz 17:51ac4252fa1a 655 } while(option != 40);
icyzkungz 12:8d773bc149b2 656
icyzkungz 11:fceaaa7e120a 657 }
icyzkungz 11:fceaaa7e120a 658
icyzkungz 17:51ac4252fa1a 659 void getData()
icyzkungz 11:fceaaa7e120a 660 {
icyzkungz 17:51ac4252fa1a 661
icyzkungz 17:51ac4252fa1a 662 }
icyzkungz 11:fceaaa7e120a 663
icyzkungz 11:fceaaa7e120a 664
onehorse 0:2e5e65a6fb30 665 int main()
onehorse 0:2e5e65a6fb30 666 {
icyzkungz 11:fceaaa7e120a 667 pc.baud(115200);
icyzkungz 17:51ac4252fa1a 668
icyzkungz 17:51ac4252fa1a 669 if(!button) { //Set up mode
icyzkungz 17:51ac4252fa1a 670 while(!button);
icyzkungz 17:51ac4252fa1a 671 pc.printf("Switch Mode\n");
icyzkungz 17:51ac4252fa1a 672 SwMode();
icyzkungz 17:51ac4252fa1a 673 }
icyzkungz 11:fceaaa7e120a 674
icyzkungz 11:fceaaa7e120a 675 /*while(1){
icyzkungz 11:fceaaa7e120a 676 Kinematic test('P',10,15,10,10);
icyzkungz 11:fceaaa7e120a 677 pc.printf("\n\nLink Hip : %f, Link Knee : %f, Position Y : %f, Position Z : %f\n",test.get_Link_Hip(),test.get_Link_Knee(),test.get_Position_Y(),test.get_Position_Z());
icyzkungz 11:fceaaa7e120a 678 pc.printf("Zeta Hip : %f, Zeta Knee : %f\n",test.get_Zeta_Hip(),test.get_Zeta_Knee());
icyzkungz 11:fceaaa7e120a 679 test.set_Link_Hip(25);
icyzkungz 11:fceaaa7e120a 680 test.set_Link_Knee(30);
icyzkungz 11:fceaaa7e120a 681 test.set_Position_Y(35);
icyzkungz 11:fceaaa7e120a 682 test.set_Position_Z(40);
icyzkungz 11:fceaaa7e120a 683 test.set_Zeta_Hip(45);
icyzkungz 11:fceaaa7e120a 684 test.set_Zeta_Knee(50);
icyzkungz 11:fceaaa7e120a 685 pc.printf("\nLink Hip : %f, Link Knee : %f, Position Y : %f, Position Z : %f\n",test.get_Link_Hip(),test.get_Link_Knee(),test.get_Position_Y(),test.get_Position_Z());
icyzkungz 11:fceaaa7e120a 686 pc.printf("Zeta Hip : %f, Zeta Knee : %f\n",test.get_Zeta_Hip(),test.get_Zeta_Knee());
icyzkungz 11:fceaaa7e120a 687
icyzkungz 11:fceaaa7e120a 688 while(1);
icyzkungz 11:fceaaa7e120a 689 }*/
icyzkungz 6:fa132fd505ce 690
soulx 3:3e04c1c03cab 691
soulx 3:3e04c1c03cab 692
soulx 3:3e04c1c03cab 693 //Set up I2C
soulx 3:3e04c1c03cab 694 i2c.frequency(400000); // use fast (400 kHz) I2C
onehorse 0:2e5e65a6fb30 695
soulx 3:3e04c1c03cab 696 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
soulx 3:3e04c1c03cab 697
soulx 3:3e04c1c03cab 698 t.start();
soulx 3:3e04c1c03cab 699
soulx 3:3e04c1c03cab 700 //mpu9250.resetMPU9250();
soulx 3:3e04c1c03cab 701 // Read the WHO_AM_I register, this is a good test of communication
soulx 3:3e04c1c03cab 702 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
soulx 3:3e04c1c03cab 703 pc.printf("I AM 0x%x\n\r", whoami);
soulx 3:3e04c1c03cab 704 pc.printf("I SHOULD BE 0x71\n\r");
soulx 3:3e04c1c03cab 705
soulx 3:3e04c1c03cab 706 if (whoami == 0x71) { // WHO_AM_I should always be 0x68
soulx 3:3e04c1c03cab 707 pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
soulx 3:3e04c1c03cab 708 pc.printf("MPU9250 is online...\n\r");
soulx 3:3e04c1c03cab 709 sprintf(buffer, "0x%x", whoami);
soulx 3:3e04c1c03cab 710 wait(1);
soulx 3:3e04c1c03cab 711
soulx 3:3e04c1c03cab 712 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
soulx 3:3e04c1c03cab 713 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
soulx 3:3e04c1c03cab 714 pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
soulx 3:3e04c1c03cab 715 pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
soulx 3:3e04c1c03cab 716 pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
soulx 3:3e04c1c03cab 717 pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
soulx 3:3e04c1c03cab 718 pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
soulx 3:3e04c1c03cab 719 pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
soulx 3:3e04c1c03cab 720 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
soulx 3:3e04c1c03cab 721 pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
soulx 3:3e04c1c03cab 722 pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
soulx 3:3e04c1c03cab 723 pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
soulx 3:3e04c1c03cab 724 pc.printf("x accel bias = %f\n\r", accelBias[0]);
soulx 3:3e04c1c03cab 725 pc.printf("y accel bias = %f\n\r", accelBias[1]);
soulx 3:3e04c1c03cab 726 pc.printf("z accel bias = %f\n\r", accelBias[2]);
soulx 3:3e04c1c03cab 727 wait(2);
soulx 3:3e04c1c03cab 728 mpu9250.initMPU9250();
soulx 3:3e04c1c03cab 729 pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
soulx 3:3e04c1c03cab 730 mpu9250.initAK8963(magCalibration);
soulx 3:3e04c1c03cab 731 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
soulx 3:3e04c1c03cab 732
soulx 3:3e04c1c03cab 733 whoami = mpu9250.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); // Read WHO_AM_I register for MPU-9250
soulx 3:3e04c1c03cab 734 pc.printf("I AM 0x%x\n\r", whoami);
soulx 3:3e04c1c03cab 735 pc.printf("I SHOULD BE 0x48\n\r");
soulx 3:3e04c1c03cab 736 if(whoami != 0x48) {
soulx 3:3e04c1c03cab 737 while(1);
soulx 3:3e04c1c03cab 738 }
soulx 3:3e04c1c03cab 739 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
soulx 3:3e04c1c03cab 740 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
soulx 3:3e04c1c03cab 741 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
soulx 3:3e04c1c03cab 742 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
soulx 3:3e04c1c03cab 743 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
soulx 3:3e04c1c03cab 744 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
soulx 3:3e04c1c03cab 745 wait(1);
soulx 3:3e04c1c03cab 746 } else {
soulx 3:3e04c1c03cab 747 pc.printf("Could not connect to MPU9250: \n\r");
soulx 3:3e04c1c03cab 748 pc.printf("%#x \n", whoami);
soulx 3:3e04c1c03cab 749
soulx 3:3e04c1c03cab 750 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
soulx 3:3e04c1c03cab 751
soulx 3:3e04c1c03cab 752 while(1) ; // Loop forever if communication doesn't happen
onehorse 0:2e5e65a6fb30 753 }
onehorse 0:2e5e65a6fb30 754
onehorse 0:2e5e65a6fb30 755 mpu9250.getAres(); // Get accelerometer sensitivity
onehorse 0:2e5e65a6fb30 756 mpu9250.getGres(); // Get gyro sensitivity
onehorse 0:2e5e65a6fb30 757 mpu9250.getMres(); // Get magnetometer sensitivity
onehorse 0:2e5e65a6fb30 758 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
onehorse 0:2e5e65a6fb30 759 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
onehorse 0:2e5e65a6fb30 760 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
icyzkungz 6:fa132fd505ce 761
icyzkungz 6:fa132fd505ce 762 /*pc.printf("START scan mag\n\r\n\r\n\r");
icyzkungz 6:fa132fd505ce 763 //wait(1);
icyzkungz 6:fa132fd505ce 764 for(int i=0; i<4000; i++) {
icyzkungz 6:fa132fd505ce 765 mpu9250.readMagData(magCount);
soulx 3:3e04c1c03cab 766
icyzkungz 6:fa132fd505ce 767 if(magCount[0]<xmin)
icyzkungz 6:fa132fd505ce 768 xmin = magCount[0];
icyzkungz 6:fa132fd505ce 769 if(magCount[0]>xmax)
icyzkungz 6:fa132fd505ce 770 xmax = magCount[0];
icyzkungz 6:fa132fd505ce 771
icyzkungz 6:fa132fd505ce 772 if(magCount[1]<ymin)
icyzkungz 6:fa132fd505ce 773 ymin = magCount[1];
icyzkungz 6:fa132fd505ce 774 if(magCount[1]>ymax)
icyzkungz 6:fa132fd505ce 775 ymax = magCount[1];
icyzkungz 6:fa132fd505ce 776
icyzkungz 6:fa132fd505ce 777 if(magCount[2]<zmin)
icyzkungz 6:fa132fd505ce 778 zmin = magCount[2];
icyzkungz 6:fa132fd505ce 779 if(magCount[2]>zmax)
icyzkungz 6:fa132fd505ce 780 zmax = magCount[2];
soulx 3:3e04c1c03cab 781
soulx 3:3e04c1c03cab 782
icyzkungz 6:fa132fd505ce 783 wait_ms(10);
soulx 3:3e04c1c03cab 784 }
soulx 3:3e04c1c03cab 785 pc.printf("FINISH scan\r\n\r\n");
soulx 3:3e04c1c03cab 786 pc.printf("Mx Max= %f Min= %f\n\r",xmax,xmin);
soulx 3:3e04c1c03cab 787 pc.printf("My Max= %f Min= %f\n\r",ymax,ymin);
icyzkungz 6:fa132fd505ce 788 pc.printf("Mz Max= %f Min= %f\n\r",zmax,zmin);*/
icyzkungz 6:fa132fd505ce 789
icyzkungz 6:fa132fd505ce 790 /*xmax = 188.000000;
icyzkungz 6:fa132fd505ce 791 xmin = -316.000000;
icyzkungz 6:fa132fd505ce 792 ymax = 485.000000;
icyzkungz 6:fa132fd505ce 793 ymin = -26.000000;
icyzkungz 6:fa132fd505ce 794 zmax = 165.000000;
icyzkungz 6:fa132fd505ce 795 xmin = -230.000000;
soulx 3:3e04c1c03cab 796
icyzkungz 6:fa132fd505ce 797 //Ice room
icyzkungz 6:fa132fd505ce 798 xmax = 101.000000;
icyzkungz 6:fa132fd505ce 799 xmin = -296.000000;
icyzkungz 6:fa132fd505ce 800 ymax = 320.000000;
icyzkungz 6:fa132fd505ce 801 ymin = -85.000000;
icyzkungz 6:fa132fd505ce 802 zmax = 208.000000;
icyzkungz 6:fa132fd505ce 803 xmin = -202.000000;
icyzkungz 6:fa132fd505ce 804
icyzkungz 6:fa132fd505ce 805 xmax = 115.000000;
icyzkungz 6:fa132fd505ce 806 xmin = -309.000000;
icyzkungz 6:fa132fd505ce 807 ymax = 350.000000;
icyzkungz 6:fa132fd505ce 808 ymin = -119.000000;
icyzkungz 6:fa132fd505ce 809 zmax = 235.000000;
icyzkungz 6:fa132fd505ce 810 zmin = -224.000000;*/
icyzkungz 6:fa132fd505ce 811
icyzkungz 6:fa132fd505ce 812 xmax = 120.000000;
icyzkungz 6:fa132fd505ce 813 xmin = -306.000000;
icyzkungz 6:fa132fd505ce 814 ymax = 340.000000;
icyzkungz 6:fa132fd505ce 815 ymin = -90.000000;
icyzkungz 6:fa132fd505ce 816 zmax = 219.000000;
icyzkungz 6:fa132fd505ce 817 zmin = -195.000000;
soulx 3:3e04c1c03cab 818
soulx 3:3e04c1c03cab 819
soulx 3:3e04c1c03cab 820
soulx 4:1e5db958fd1b 821 magbias[0] = -1.0;
soulx 4:1e5db958fd1b 822 magbias[1] = -1.0;
soulx 4:1e5db958fd1b 823 magbias[2] = -1.0;
soulx 4:1e5db958fd1b 824
soulx 4:1e5db958fd1b 825 magCalibration[0] = 2.0f / (xmax -xmin);
soulx 4:1e5db958fd1b 826 magCalibration[1] = 2.0f / (ymax -ymin);
soulx 4:1e5db958fd1b 827 magCalibration[2] = 2.0f / (zmax -zmin);
soulx 4:1e5db958fd1b 828
soulx 3:3e04c1c03cab 829 //magbias[0] = (xmin-xmax)/2.0f; // User environmental x-axis correction in milliGauss, should be automatically calculated
soulx 3:3e04c1c03cab 830 //magbias[1] = (ymin-ymax)/2.0f; // User environmental x-axis correction in milliGauss
soulx 3:3e04c1c03cab 831 //magbias[2] = (zmin-zmax)/2.0f; // User environmental x-axis correction in milliGauss
icyzkungz 6:fa132fd505ce 832 pc.printf("mag[0] %f",magbias[0]);
icyzkungz 6:fa132fd505ce 833 pc.printf("mag[1] %f",magbias[1]);
icyzkungz 6:fa132fd505ce 834 pc.printf("mag[2] %f\n\r",magbias[2]);
soulx 3:3e04c1c03cab 835 // resalt = atan(magY+((yMin-yMax)/2),magX+(xMin-xMax)/2))*180/PI;
onehorse 0:2e5e65a6fb30 836
soulx 3:3e04c1c03cab 837
icyzkungz 6:fa132fd505ce 838 float temp_time;
icyzkungz 6:fa132fd505ce 839
soulx 3:3e04c1c03cab 840 while(1) {
icyzkungz 6:fa132fd505ce 841 temp_time = t.read();
soulx 3:3e04c1c03cab 842 // If intPin goes high, all data registers have new data
soulx 3:3e04c1c03cab 843 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
soulx 3:3e04c1c03cab 844
soulx 3:3e04c1c03cab 845 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
soulx 3:3e04c1c03cab 846 // Now we'll calculate the accleration value into actual g's
soulx 3:3e04c1c03cab 847 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
soulx 3:3e04c1c03cab 848 ay = (float)accelCount[1]*aRes - accelBias[1];
soulx 3:3e04c1c03cab 849 az = (float)accelCount[2]*aRes - accelBias[2];
soulx 3:3e04c1c03cab 850
soulx 3:3e04c1c03cab 851 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
soulx 3:3e04c1c03cab 852 // Calculate the gyro value into actual degrees per second
soulx 3:3e04c1c03cab 853 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
soulx 3:3e04c1c03cab 854 gy = (float)gyroCount[1]*gRes - gyroBias[1];
soulx 3:3e04c1c03cab 855 gz = (float)gyroCount[2]*gRes - gyroBias[2];
soulx 3:3e04c1c03cab 856
soulx 3:3e04c1c03cab 857 mpu9250.readMagData(magCount); // Read the x/y/z adc values
soulx 3:3e04c1c03cab 858 // Calculate the magnetometer values in milliGauss
soulx 3:3e04c1c03cab 859 // Include factory calibration per data sheet and user environmental corrections
icyzkungz 6:fa132fd505ce 860 /* if(magCount[0]<xmin)
icyzkungz 6:fa132fd505ce 861 xmin = magCount[0];
icyzkungz 6:fa132fd505ce 862 if(magCount[0]>xmax)
icyzkungz 6:fa132fd505ce 863 xmax = magCount[0];
soulx 3:3e04c1c03cab 864
icyzkungz 6:fa132fd505ce 865 if(magCount[1]<ymin)
icyzkungz 6:fa132fd505ce 866 ymin = magCount[1];
icyzkungz 6:fa132fd505ce 867 if(magCount[1]>ymax)
icyzkungz 6:fa132fd505ce 868 ymax = magCount[1];
soulx 3:3e04c1c03cab 869
icyzkungz 6:fa132fd505ce 870 if(magCount[2]<zmin)
icyzkungz 6:fa132fd505ce 871 zmin = magCount[2];
icyzkungz 6:fa132fd505ce 872 if(mz>zmax)
icyzkungz 6:fa132fd505ce 873 zmax = mz;
icyzkungz 6:fa132fd505ce 874 wait_ms(1);
icyzkungz 6:fa132fd505ce 875 */
icyzkungz 6:fa132fd505ce 876 // pc.printf("FINISH scan\r\n\r\n");
soulx 3:3e04c1c03cab 877
soulx 4:1e5db958fd1b 878 // mx = (float)magCount[0]*mRes*magCalibration[0] + magbias[0]; // get actual magnetometer value, this depends on scale being set
soulx 4:1e5db958fd1b 879 // my = (float)magCount[1]*mRes*magCalibration[1] + magbias[1];
soulx 4:1e5db958fd1b 880 // mz = (float)magCount[2]*mRes*magCalibration[2] + magbias[2];
soulx 4:1e5db958fd1b 881
soulx 4:1e5db958fd1b 882 mx = ((float)magCount[0]-xmin)*magCalibration[0] + magbias[0]; // get actual magnetometer value, this depends on scale being set
soulx 4:1e5db958fd1b 883 my = ((float)magCount[1]-ymin)*magCalibration[1] + magbias[1];
soulx 4:1e5db958fd1b 884 mz = ((float)magCount[2]-zmin)*magCalibration[2] + magbias[2];
soulx 3:3e04c1c03cab 885
soulx 3:3e04c1c03cab 886 // mx = (float)magCount[0]*1.499389499f - magbias[0]; // get actual magnetometer value, this depends on scale being set
soulx 3:3e04c1c03cab 887 // my = (float)magCount[1]*1.499389499f - magbias[1];
soulx 3:3e04c1c03cab 888 // mz = (float)magCount[2]*1.499389499f - magbias[2];
soulx 3:3e04c1c03cab 889
soulx 3:3e04c1c03cab 890
soulx 3:3e04c1c03cab 891
soulx 3:3e04c1c03cab 892
soulx 3:3e04c1c03cab 893 }
soulx 3:3e04c1c03cab 894
soulx 3:3e04c1c03cab 895 Now = t.read_us();
soulx 3:3e04c1c03cab 896 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
soulx 3:3e04c1c03cab 897 lastUpdate = Now;
soulx 3:3e04c1c03cab 898
soulx 3:3e04c1c03cab 899 sum += deltat;
soulx 3:3e04c1c03cab 900 sumCount++;
soulx 3:3e04c1c03cab 901
onehorse 0:2e5e65a6fb30 902 // if(lastUpdate - firstUpdate > 10000000.0f) {
onehorse 0:2e5e65a6fb30 903 // beta = 0.04; // decrease filter gain after stabilized
onehorse 0:2e5e65a6fb30 904 // zeta = 0.015; // increasey bias drift gain after stabilized
soulx 3:3e04c1c03cab 905 // }
soulx 3:3e04c1c03cab 906
soulx 3:3e04c1c03cab 907 // Pass gyro rate as rad/s
icyzkungz 6:fa132fd505ce 908 //mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
icyzkungz 6:fa132fd505ce 909 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
soulx 3:3e04c1c03cab 910
soulx 3:3e04c1c03cab 911 // Serial print and/or display at 0.5 s rate independent of data rates
soulx 3:3e04c1c03cab 912 delt_t = t.read_ms() - count;
icyzkungz 6:fa132fd505ce 913 if(temp_time > 8) {
icyzkungz 6:fa132fd505ce 914 if (delt_t > 500) { // update LCD once per half-second independent of read rate
soulx 3:3e04c1c03cab 915
icyzkungz 6:fa132fd505ce 916 /*pc.printf("ax = %f", 1000*ax);
icyzkungz 6:fa132fd505ce 917 pc.printf(" ay = %f", 1000*ay);
icyzkungz 6:fa132fd505ce 918 pc.printf(" az = %f mg\n\r", 1000*az);
onehorse 0:2e5e65a6fb30 919
icyzkungz 6:fa132fd505ce 920 pc.printf("gx = %f", gx);
icyzkungz 6:fa132fd505ce 921 pc.printf(" gy = %f", gy);
icyzkungz 6:fa132fd505ce 922 pc.printf(" gz = %f deg/s\n\r", gz);
soulx 3:3e04c1c03cab 923
icyzkungz 6:fa132fd505ce 924 pc.printf("mx = %f", mx);
icyzkungz 6:fa132fd505ce 925 pc.printf(" my = %f", my);
icyzkungz 6:fa132fd505ce 926 pc.printf(" mz = %f mG\n\r", mz);*/
onehorse 0:2e5e65a6fb30 927
icyzkungz 6:fa132fd505ce 928 whoami = mpu9250.readByte(AK8963_ADDRESS, AK8963_ST2); // Read WHO_AM_I register for MPU-9250
icyzkungz 6:fa132fd505ce 929 // pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x10\n\r");
icyzkungz 6:fa132fd505ce 930 if(whoami == 0x14) {
icyzkungz 6:fa132fd505ce 931 pc.printf("I AM 0x%x\n\r", whoami);
icyzkungz 6:fa132fd505ce 932 while(1);
icyzkungz 6:fa132fd505ce 933 }
soulx 3:3e04c1c03cab 934
soulx 3:3e04c1c03cab 935
icyzkungz 6:fa132fd505ce 936 tempCount = mpu9250.readTempData(); // Read the adc values
icyzkungz 6:fa132fd505ce 937 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
icyzkungz 6:fa132fd505ce 938 //pc.printf(" temperature = %f C\n\r", temperature);
onehorse 0:2e5e65a6fb30 939
icyzkungz 6:fa132fd505ce 940 // pc.printf("q0 = %f\n\r", q[0]);
icyzkungz 6:fa132fd505ce 941 // pc.printf("q1 = %f\n\r", q[1]);
icyzkungz 6:fa132fd505ce 942 // pc.printf("q2 = %f\n\r", q[2]);
icyzkungz 6:fa132fd505ce 943 // pc.printf("q3 = %f\n\r", q[3]);
onehorse 0:2e5e65a6fb30 944
icyzkungz 6:fa132fd505ce 945 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
icyzkungz 6:fa132fd505ce 946 // In this coordinate system, the positive z-axis is down toward Earth.
icyzkungz 6:fa132fd505ce 947 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
icyzkungz 6:fa132fd505ce 948 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
icyzkungz 6:fa132fd505ce 949 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
icyzkungz 6:fa132fd505ce 950 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
icyzkungz 6:fa132fd505ce 951 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
icyzkungz 6:fa132fd505ce 952 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
icyzkungz 6:fa132fd505ce 953 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
icyzkungz 6:fa132fd505ce 954 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
icyzkungz 6:fa132fd505ce 955 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
icyzkungz 6:fa132fd505ce 956 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
soulx 3:3e04c1c03cab 957
icyzkungz 6:fa132fd505ce 958 float Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch);
icyzkungz 6:fa132fd505ce 959 float Yh = my*cos(roll)+mz*sin(roll);
soulx 3:3e04c1c03cab 960
icyzkungz 6:fa132fd505ce 961 float yawmag = atan2(Yh,Xh)+PI;
icyzkungz 6:fa132fd505ce 962 //pc.printf("Xh= %f Yh= %f ",Xh,Yh);
icyzkungz 6:fa132fd505ce 963 //pc.printf("Yaw[mag]= %f\n\r",yawmag*180.0f/PI);
soulx 3:3e04c1c03cab 964
soulx 3:3e04c1c03cab 965
soulx 3:3e04c1c03cab 966
icyzkungz 6:fa132fd505ce 967 pitch *= 180.0f / PI;
icyzkungz 6:fa132fd505ce 968 yaw *= 180.0f / PI;
icyzkungz 6:fa132fd505ce 969 yaw += 180.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
icyzkungz 6:fa132fd505ce 970 roll *= 180.0f / PI;
icyzkungz 6:fa132fd505ce 971
icyzkungz 6:fa132fd505ce 972 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
icyzkungz 6:fa132fd505ce 973 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
icyzkungz 6:fa132fd505ce 974
icyzkungz 6:fa132fd505ce 975
soulx 19:5b8d16378d66 976 //WheelChair();
soulx 3:3e04c1c03cab 977
soulx 3:3e04c1c03cab 978
icyzkungz 6:fa132fd505ce 979 myled= !myled;
icyzkungz 6:fa132fd505ce 980 count = t.read_ms();
onehorse 0:2e5e65a6fb30 981
icyzkungz 6:fa132fd505ce 982 if(count > 1<<21) {
icyzkungz 6:fa132fd505ce 983 t.start(); // start the timer over again if ~30 minutes has passed
icyzkungz 6:fa132fd505ce 984 count = 0;
icyzkungz 6:fa132fd505ce 985 deltat= 0;
icyzkungz 6:fa132fd505ce 986 lastUpdate = t.read_us();
icyzkungz 6:fa132fd505ce 987 }
icyzkungz 6:fa132fd505ce 988 sum = 0;
icyzkungz 6:fa132fd505ce 989 sumCount = 0;
soulx 3:3e04c1c03cab 990 }
soulx 3:3e04c1c03cab 991 }
onehorse 0:2e5e65a6fb30 992 }
onehorse 0:2e5e65a6fb30 993 }