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