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