turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
main.cpp@1:13164a15fbf6, 2018-04-19 (annotated)
- Committer:
- Khanchana
- Date:
- Thu Apr 19 01:54:55 2018 +0000
- Revision:
- 1:13164a15fbf6
- Parent:
- 0:43d21d5145d3
- Child:
- 2:18835f8732ad
alpha 0.8; beta 0.5
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Khanchana | 0:43d21d5145d3 | 1 | #include "mbed.h" |
Khanchana | 0:43d21d5145d3 | 2 | #include "Servo.h" |
Khanchana | 0:43d21d5145d3 | 3 | #include "rtos.h" |
Khanchana | 0:43d21d5145d3 | 4 | #include "attitude.h" |
Khanchana | 1:13164a15fbf6 | 5 | #include "math.h" |
Khanchana | 0:43d21d5145d3 | 6 | |
Khanchana | 0:43d21d5145d3 | 7 | Serial pc(USBTX, USBRX); |
Khanchana | 0:43d21d5145d3 | 8 | |
Khanchana | 0:43d21d5145d3 | 9 | Timer timer1; |
Khanchana | 0:43d21d5145d3 | 10 | Timer timerwalk; |
Khanchana | 0:43d21d5145d3 | 11 | |
Khanchana | 0:43d21d5145d3 | 12 | Thread thread1; |
Khanchana | 0:43d21d5145d3 | 13 | Thread thread2; |
Khanchana | 0:43d21d5145d3 | 14 | |
Khanchana | 0:43d21d5145d3 | 15 | Servo Servo1(D6); |
Khanchana | 0:43d21d5145d3 | 16 | Servo Servo2(D8); |
Khanchana | 0:43d21d5145d3 | 17 | Servo Servo3(D9); |
Khanchana | 0:43d21d5145d3 | 18 | Servo Servo4(D10); |
Khanchana | 0:43d21d5145d3 | 19 | |
Khanchana | 0:43d21d5145d3 | 20 | void IMU(); |
Khanchana | 0:43d21d5145d3 | 21 | void Cal_si(); |
Khanchana | 0:43d21d5145d3 | 22 | void Avg(); |
Khanchana | 1:13164a15fbf6 | 23 | void Cal_Cg_linear(); |
Khanchana | 1:13164a15fbf6 | 24 | void Cal_Cg_log(); |
Khanchana | 0:43d21d5145d3 | 25 | void move(); |
Khanchana | 0:43d21d5145d3 | 26 | void receive_hormone(); |
Khanchana | 0:43d21d5145d3 | 27 | void cal_step_down(); //calculate step |
Khanchana | 0:43d21d5145d3 | 28 | void cal_step_up(); |
Khanchana | 0:43d21d5145d3 | 29 | void servo(); |
Khanchana | 0:43d21d5145d3 | 30 | void servo_Right(); |
Khanchana | 0:43d21d5145d3 | 31 | |
Khanchana | 0:43d21d5145d3 | 32 | int walking_time; |
Khanchana | 0:43d21d5145d3 | 33 | int i = 0; |
Khanchana | 0:43d21d5145d3 | 34 | |
Khanchana | 0:43d21d5145d3 | 35 | float avg = 0.00; |
Khanchana | 0:43d21d5145d3 | 36 | float sum = 0.00; |
Khanchana | 0:43d21d5145d3 | 37 | float roll_data[10]; |
Khanchana | 0:43d21d5145d3 | 38 | float Si = 0.00 ; |
Khanchana | 0:43d21d5145d3 | 39 | float Cg = 0.00; |
Khanchana | 1:13164a15fbf6 | 40 | float Cg_down = 0.00; |
Khanchana | 1:13164a15fbf6 | 41 | float Cg_up = 0; |
Khanchana | 1:13164a15fbf6 | 42 | float standardDeviation = 0; |
Khanchana | 1:13164a15fbf6 | 43 | float sd = 0; |
Khanchana | 0:43d21d5145d3 | 44 | |
Khanchana | 0:43d21d5145d3 | 45 | float pos_down_start = 1400.00; |
Khanchana | 0:43d21d5145d3 | 46 | float pos_up_start = 1000.00; |
Khanchana | 0:43d21d5145d3 | 47 | float down_degree = 90.00; |
Khanchana | 0:43d21d5145d3 | 48 | float up_degree = 45.00; |
Khanchana | 0:43d21d5145d3 | 49 | float stepmin = 1; |
Khanchana | 1:13164a15fbf6 | 50 | float round = 5; |
Khanchana | 0:43d21d5145d3 | 51 | float waittime = 0.001 ; |
Khanchana | 0:43d21d5145d3 | 52 | |
Khanchana | 0:43d21d5145d3 | 53 | float pos_down_left = 1400.00; |
Khanchana | 0:43d21d5145d3 | 54 | float pos_up_left = 1000.00; |
Khanchana | 0:43d21d5145d3 | 55 | float pos_down_end_left; |
Khanchana | 0:43d21d5145d3 | 56 | float pos_up_end_left; |
Khanchana | 0:43d21d5145d3 | 57 | float state_count_left = 1; |
Khanchana | 0:43d21d5145d3 | 58 | float round_count_left = 1; |
Khanchana | 0:43d21d5145d3 | 59 | float step_down_left; |
Khanchana | 0:43d21d5145d3 | 60 | float step_up_left; |
Khanchana | 0:43d21d5145d3 | 61 | |
Khanchana | 0:43d21d5145d3 | 62 | float pos_down_right = 1400.00; |
Khanchana | 0:43d21d5145d3 | 63 | float pos_up_right = 1000.00; |
Khanchana | 0:43d21d5145d3 | 64 | float pos_down_end_right; |
Khanchana | 0:43d21d5145d3 | 65 | float pos_up_end_right; |
Khanchana | 0:43d21d5145d3 | 66 | float state_count_right = 1; |
Khanchana | 0:43d21d5145d3 | 67 | float round_count_right = 1; |
Khanchana | 0:43d21d5145d3 | 68 | float step_up_right; |
Khanchana | 0:43d21d5145d3 | 69 | float step_down_right; |
Khanchana | 0:43d21d5145d3 | 70 | |
Khanchana | 0:43d21d5145d3 | 71 | |
Khanchana | 0:43d21d5145d3 | 72 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 73 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 74 | // |
Khanchana | 0:43d21d5145d3 | 75 | int main() { // |
Khanchana | 0:43d21d5145d3 | 76 | pc.baud(1000000); // |
Khanchana | 0:43d21d5145d3 | 77 | timer1.start(); // start timer counting // |
Khanchana | 0:43d21d5145d3 | 78 | pc.printf("PRESS '1'\n"); // |
Khanchana | 0:43d21d5145d3 | 79 | if (pc.getc() == '1'){ // |
Khanchana | 0:43d21d5145d3 | 80 | thread2.start(servo); // |
Khanchana | 0:43d21d5145d3 | 81 | } // |
Khanchana | 0:43d21d5145d3 | 82 | } // |
Khanchana | 0:43d21d5145d3 | 83 | // |
Khanchana | 0:43d21d5145d3 | 84 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 85 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 86 | |
Khanchana | 0:43d21d5145d3 | 87 | ///////////////////////////Control SERVO//////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 88 | //////////////////////////////////////////////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 89 | void servo() { |
Khanchana | 0:43d21d5145d3 | 90 | pc.printf("start\n"); |
Khanchana | 0:43d21d5145d3 | 91 | attitude_setup(); |
Khanchana | 0:43d21d5145d3 | 92 | thread1.start(IMU); |
Khanchana | 0:43d21d5145d3 | 93 | //wait(0.01); |
Khanchana | 0:43d21d5145d3 | 94 | timerwalk.start(); // start timer counting |
Khanchana | 0:43d21d5145d3 | 95 | move(); |
Khanchana | 0:43d21d5145d3 | 96 | //pc.printf("%f\n",roll_min); |
Khanchana | 0:43d21d5145d3 | 97 | //pc.printf("%f\n",roll_max); |
Khanchana | 0:43d21d5145d3 | 98 | } |
Khanchana | 0:43d21d5145d3 | 99 | |
Khanchana | 0:43d21d5145d3 | 100 | void receive_hormone(){ |
Khanchana | 1:13164a15fbf6 | 101 | down_degree = 90.00*(1.00-(0.06*Cg_down)); |
Khanchana | 1:13164a15fbf6 | 102 | /*pc.printf("%f \t",Si); |
Khanchana | 1:13164a15fbf6 | 103 | pc.printf("down\t"); |
Khanchana | 1:13164a15fbf6 | 104 | pc.printf("%f \t",down_degree); |
Khanchana | 1:13164a15fbf6 | 105 | pc.printf("%f\t",Cg);*/ |
Khanchana | 1:13164a15fbf6 | 106 | up_degree = 45.00*(1.00+(0.7*Cg_up)); |
Khanchana | 1:13164a15fbf6 | 107 | /*pc.printf("up\t"); |
Khanchana | 1:13164a15fbf6 | 108 | pc.printf("%f \t",up_degree); |
Khanchana | 1:13164a15fbf6 | 109 | pc.printf("%f\n",Cg);*/ |
Khanchana | 1:13164a15fbf6 | 110 | if(down_degree < 85){ |
Khanchana | 1:13164a15fbf6 | 111 | down_degree = 85; |
Khanchana | 0:43d21d5145d3 | 112 | if(up_degree > 75){ |
Khanchana | 0:43d21d5145d3 | 113 | up_degree = 75; |
Khanchana | 0:43d21d5145d3 | 114 | } |
Khanchana | 0:43d21d5145d3 | 115 | } |
Khanchana | 0:43d21d5145d3 | 116 | } |
Khanchana | 0:43d21d5145d3 | 117 | |
Khanchana | 0:43d21d5145d3 | 118 | void cal_step_down(){ |
Khanchana | 0:43d21d5145d3 | 119 | //pc.printf("down"); |
Khanchana | 0:43d21d5145d3 | 120 | //pc.printf("%f \n",down_degree); |
Khanchana | 0:43d21d5145d3 | 121 | pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); |
Khanchana | 0:43d21d5145d3 | 122 | pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree))); |
Khanchana | 0:43d21d5145d3 | 123 | if (pos_down_end_right > pos_down_end_left){ |
Khanchana | 0:43d21d5145d3 | 124 | step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start); |
Khanchana | 0:43d21d5145d3 | 125 | step_down_left = stepmin; |
Khanchana | 0:43d21d5145d3 | 126 | } else if (pos_down_end_right < pos_down_end_left){ |
Khanchana | 0:43d21d5145d3 | 127 | step_down_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 128 | step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start); |
Khanchana | 0:43d21d5145d3 | 129 | } else{ |
Khanchana | 0:43d21d5145d3 | 130 | step_down_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 131 | step_down_left = stepmin; |
Khanchana | 0:43d21d5145d3 | 132 | } |
Khanchana | 0:43d21d5145d3 | 133 | /*pc.printf("pos_down_right"); |
Khanchana | 0:43d21d5145d3 | 134 | pc.printf("%f\n",pos_down_end_right); |
Khanchana | 0:43d21d5145d3 | 135 | pc.printf("pos_down_left"); |
Khanchana | 0:43d21d5145d3 | 136 | pc.printf("%f\n",pos_down_end_left); |
Khanchana | 0:43d21d5145d3 | 137 | pc.printf("step_down_right"); |
Khanchana | 0:43d21d5145d3 | 138 | pc.printf("%f\n",step_down_right); |
Khanchana | 0:43d21d5145d3 | 139 | pc.printf("step_down_left"); |
Khanchana | 0:43d21d5145d3 | 140 | pc.printf("%f\n",step_down_left);*/ |
Khanchana | 0:43d21d5145d3 | 141 | } |
Khanchana | 0:43d21d5145d3 | 142 | |
Khanchana | 0:43d21d5145d3 | 143 | void cal_step_up(){ |
Khanchana | 0:43d21d5145d3 | 144 | //pc.printf("up"); |
Khanchana | 0:43d21d5145d3 | 145 | //pc.printf("%f \n",up_degree); |
Khanchana | 0:43d21d5145d3 | 146 | pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); |
Khanchana | 0:43d21d5145d3 | 147 | pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); |
Khanchana | 0:43d21d5145d3 | 148 | if (pos_up_end_right > pos_up_end_left){ |
Khanchana | 0:43d21d5145d3 | 149 | step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start); |
Khanchana | 0:43d21d5145d3 | 150 | step_up_left = stepmin; |
Khanchana | 0:43d21d5145d3 | 151 | } else if (pos_up_end_right < pos_up_end_left){ |
Khanchana | 0:43d21d5145d3 | 152 | step_up_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 153 | step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start); |
Khanchana | 0:43d21d5145d3 | 154 | } else{ |
Khanchana | 0:43d21d5145d3 | 155 | step_up_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 156 | step_up_left = stepmin; |
Khanchana | 0:43d21d5145d3 | 157 | } |
Khanchana | 0:43d21d5145d3 | 158 | /*pc.printf("pos_up_right"); |
Khanchana | 0:43d21d5145d3 | 159 | pc.printf("%f\n",pos_up_end_right); |
Khanchana | 0:43d21d5145d3 | 160 | pc.printf("pos_up_left"); |
Khanchana | 0:43d21d5145d3 | 161 | pc.printf("%f\n",pos_up_end_left); |
Khanchana | 0:43d21d5145d3 | 162 | pc.printf("step_up_right"); |
Khanchana | 0:43d21d5145d3 | 163 | pc.printf("%f\n",step_up_right);; |
Khanchana | 0:43d21d5145d3 | 164 | pc.printf("step_up_left"); |
Khanchana | 0:43d21d5145d3 | 165 | pc.printf("%f\n",step_up_left); */ |
Khanchana | 0:43d21d5145d3 | 166 | } |
Khanchana | 0:43d21d5145d3 | 167 | |
Khanchana | 0:43d21d5145d3 | 168 | void move(){ |
Khanchana | 1:13164a15fbf6 | 169 | Servo1.Enable(1000,20000); |
Khanchana | 1:13164a15fbf6 | 170 | Servo2.Enable(1000,20000); |
Khanchana | 1:13164a15fbf6 | 171 | Servo3.Enable(1000,20000); |
Khanchana | 1:13164a15fbf6 | 172 | Servo4.Enable(1000,20000); |
Khanchana | 1:13164a15fbf6 | 173 | |
Khanchana | 1:13164a15fbf6 | 174 | /*pc.printf("Si\t"); |
Khanchana | 1:13164a15fbf6 | 175 | pc.printf("%f \t",Si); |
Khanchana | 1:13164a15fbf6 | 176 | pc.printf("down\t"); |
Khanchana | 1:13164a15fbf6 | 177 | pc.printf("%f \t",down_degree); |
Khanchana | 1:13164a15fbf6 | 178 | pc.printf("%f\t",Cg); |
Khanchana | 1:13164a15fbf6 | 179 | pc.printf("up\t"); |
Khanchana | 1:13164a15fbf6 | 180 | pc.printf("%f \t",up_degree); |
Khanchana | 1:13164a15fbf6 | 181 | pc.printf("%f\n",Cg);*/ |
Khanchana | 0:43d21d5145d3 | 182 | while(1) { |
Khanchana | 0:43d21d5145d3 | 183 | cal_step_down(); |
Khanchana | 0:43d21d5145d3 | 184 | cal_step_up(); |
Khanchana | 0:43d21d5145d3 | 185 | servo_Right(); |
Khanchana | 0:43d21d5145d3 | 186 | if(state_count_left == 1) { |
Khanchana | 0:43d21d5145d3 | 187 | Servo1.SetPosition(pos_down_left); |
Khanchana | 0:43d21d5145d3 | 188 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 189 | pos_down_left = pos_down_left + step_down_left; |
Khanchana | 0:43d21d5145d3 | 190 | if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) { |
Khanchana | 0:43d21d5145d3 | 191 | state_count_left = 2; |
Khanchana | 0:43d21d5145d3 | 192 | } |
Khanchana | 0:43d21d5145d3 | 193 | /*pc.printf("LAD"); |
Khanchana | 0:43d21d5145d3 | 194 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 195 | pc.printf("LAP"); |
Khanchana | 0:43d21d5145d3 | 196 | pc.printf("%f\n",pos_up_left);*/ |
Khanchana | 0:43d21d5145d3 | 197 | } else if(state_count_left == 2) { |
Khanchana | 0:43d21d5145d3 | 198 | Servo2.SetPosition(pos_up_left); |
Khanchana | 0:43d21d5145d3 | 199 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 200 | pos_up_left = pos_up_left + step_up_left; |
Khanchana | 0:43d21d5145d3 | 201 | if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) { |
Khanchana | 0:43d21d5145d3 | 202 | state_count_left = 3; |
Khanchana | 0:43d21d5145d3 | 203 | } |
Khanchana | 0:43d21d5145d3 | 204 | /*pc.printf("LBD"); |
Khanchana | 0:43d21d5145d3 | 205 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 206 | pc.printf("LBP"); |
Khanchana | 0:43d21d5145d3 | 207 | pc.printf("%f\n",pos_up_left);*/ |
Khanchana | 0:43d21d5145d3 | 208 | } else if(state_count_left == 3) { |
Khanchana | 0:43d21d5145d3 | 209 | Servo1.SetPosition(pos_down_left); |
Khanchana | 0:43d21d5145d3 | 210 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 211 | pos_down_left = pos_down_left - step_down_left; |
Khanchana | 0:43d21d5145d3 | 212 | if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) { |
Khanchana | 0:43d21d5145d3 | 213 | state_count_left = 4; |
Khanchana | 0:43d21d5145d3 | 214 | } |
Khanchana | 0:43d21d5145d3 | 215 | /*pc.printf("LCD"); |
Khanchana | 0:43d21d5145d3 | 216 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 217 | pc.printf("LCP"); |
Khanchana | 0:43d21d5145d3 | 218 | pc.printf("%f\n",pos_up_left);*/ |
Khanchana | 0:43d21d5145d3 | 219 | } else if(state_count_left == 4) { |
Khanchana | 0:43d21d5145d3 | 220 | Servo2.SetPosition(pos_up_left); |
Khanchana | 0:43d21d5145d3 | 221 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 222 | pos_up_left = pos_up_left - step_up_left; |
Khanchana | 0:43d21d5145d3 | 223 | if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) { |
Khanchana | 0:43d21d5145d3 | 224 | state_count_left = 0; |
Khanchana | 0:43d21d5145d3 | 225 | } |
Khanchana | 0:43d21d5145d3 | 226 | /*pc.printf("LDD"); |
Khanchana | 0:43d21d5145d3 | 227 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 228 | pc.printf("LDP"); |
Khanchana | 0:43d21d5145d3 | 229 | pc.printf("%f\n",pos_up_left);*/ |
Khanchana | 0:43d21d5145d3 | 230 | } else if (state_count_left == 0 and round_count_left < round) { |
Khanchana | 0:43d21d5145d3 | 231 | round_count_left = round_count_left+1; |
Khanchana | 0:43d21d5145d3 | 232 | state_count_left = 1; |
Khanchana | 0:43d21d5145d3 | 233 | pos_down_left = pos_down_start; |
Khanchana | 0:43d21d5145d3 | 234 | pos_up_left = pos_up_start; |
Khanchana | 0:43d21d5145d3 | 235 | } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round){ |
Khanchana | 0:43d21d5145d3 | 236 | thread1.terminate(); |
Khanchana | 0:43d21d5145d3 | 237 | pc.printf("Finish \n"); |
Khanchana | 0:43d21d5145d3 | 238 | walking_time = timerwalk.read_ms(); |
Khanchana | 0:43d21d5145d3 | 239 | pc.printf("Walking time = %d \n", walking_time); |
Khanchana | 0:43d21d5145d3 | 240 | break; |
Khanchana | 0:43d21d5145d3 | 241 | } |
Khanchana | 0:43d21d5145d3 | 242 | } |
Khanchana | 0:43d21d5145d3 | 243 | } |
Khanchana | 0:43d21d5145d3 | 244 | |
Khanchana | 0:43d21d5145d3 | 245 | void servo_Right() |
Khanchana | 0:43d21d5145d3 | 246 | { |
Khanchana | 0:43d21d5145d3 | 247 | if(state_count_right == 1) { |
Khanchana | 0:43d21d5145d3 | 248 | Servo3.SetPosition(pos_down_right); |
Khanchana | 0:43d21d5145d3 | 249 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 250 | pos_down_right = pos_down_right + step_down_right; |
Khanchana | 0:43d21d5145d3 | 251 | if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) { |
Khanchana | 0:43d21d5145d3 | 252 | state_count_right = 2; |
Khanchana | 0:43d21d5145d3 | 253 | } |
Khanchana | 0:43d21d5145d3 | 254 | /*pc.printf("RAD"); |
Khanchana | 0:43d21d5145d3 | 255 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 256 | pc.printf("RAP"); |
Khanchana | 0:43d21d5145d3 | 257 | pc.printf("%f\n",pos_up_right);*/ |
Khanchana | 0:43d21d5145d3 | 258 | } else if(state_count_right == 2) { |
Khanchana | 0:43d21d5145d3 | 259 | Servo4.SetPosition(pos_up_right); |
Khanchana | 0:43d21d5145d3 | 260 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 261 | pos_up_right = pos_up_right + step_up_right; |
Khanchana | 0:43d21d5145d3 | 262 | if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) { |
Khanchana | 0:43d21d5145d3 | 263 | state_count_right = 3; |
Khanchana | 0:43d21d5145d3 | 264 | } |
Khanchana | 0:43d21d5145d3 | 265 | /*pc.printf("RBD"); |
Khanchana | 0:43d21d5145d3 | 266 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 267 | pc.printf("RBP"); |
Khanchana | 0:43d21d5145d3 | 268 | pc.printf("%f\n",pos_up_right);*/ |
Khanchana | 0:43d21d5145d3 | 269 | } else if(state_count_right == 3) { |
Khanchana | 0:43d21d5145d3 | 270 | Servo3.SetPosition(pos_down_right); |
Khanchana | 0:43d21d5145d3 | 271 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 272 | pos_down_right = pos_down_right - step_down_right; |
Khanchana | 0:43d21d5145d3 | 273 | if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) { |
Khanchana | 0:43d21d5145d3 | 274 | state_count_right = 4; |
Khanchana | 0:43d21d5145d3 | 275 | } |
Khanchana | 0:43d21d5145d3 | 276 | /*pc.printf("RCD"); |
Khanchana | 0:43d21d5145d3 | 277 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 278 | pc.printf("RCP"); |
Khanchana | 0:43d21d5145d3 | 279 | pc.printf("%f\n",pos_up_right);*/ |
Khanchana | 0:43d21d5145d3 | 280 | } else if(state_count_right == 4) { |
Khanchana | 0:43d21d5145d3 | 281 | Servo4.SetPosition(pos_up_right); |
Khanchana | 0:43d21d5145d3 | 282 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 283 | pos_up_right = pos_up_right - step_up_right; |
Khanchana | 0:43d21d5145d3 | 284 | if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) { |
Khanchana | 0:43d21d5145d3 | 285 | state_count_right = 0; |
Khanchana | 0:43d21d5145d3 | 286 | } |
Khanchana | 0:43d21d5145d3 | 287 | /*pc.printf("RDD"); |
Khanchana | 0:43d21d5145d3 | 288 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 289 | pc.printf("RDP"); |
Khanchana | 0:43d21d5145d3 | 290 | pc.printf("%f\n",pos_up_right);*/ |
Khanchana | 0:43d21d5145d3 | 291 | } else if (state_count_right == 0 and round_count_right < round) { |
Khanchana | 0:43d21d5145d3 | 292 | round_count_right = round_count_right+1; |
Khanchana | 0:43d21d5145d3 | 293 | state_count_right = 1; |
Khanchana | 0:43d21d5145d3 | 294 | pos_down_right = pos_down_start; |
Khanchana | 0:43d21d5145d3 | 295 | pos_up_right = pos_up_start; |
Khanchana | 0:43d21d5145d3 | 296 | receive_hormone(); |
Khanchana | 0:43d21d5145d3 | 297 | } |
Khanchana | 0:43d21d5145d3 | 298 | } |
Khanchana | 0:43d21d5145d3 | 299 | |
Khanchana | 0:43d21d5145d3 | 300 | ///////////////////////////Control IMU//////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 301 | //////////////////////////////////////////////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 302 | void IMU(){ |
Khanchana | 1:13164a15fbf6 | 303 | pc.printf("roll\t"); |
Khanchana | 1:13164a15fbf6 | 304 | pc.printf("Si\t"); |
Khanchana | 1:13164a15fbf6 | 305 | pc.printf("Cg\t"); |
Khanchana | 1:13164a15fbf6 | 306 | pc.printf("down\t"); |
Khanchana | 1:13164a15fbf6 | 307 | pc.printf("up\n"); |
Khanchana | 1:13164a15fbf6 | 308 | |
Khanchana | 0:43d21d5145d3 | 309 | while(1) { |
Khanchana | 0:43d21d5145d3 | 310 | if (timer1.read_us() >=10000)// read time in ms |
Khanchana | 0:43d21d5145d3 | 311 | { |
Khanchana | 0:43d21d5145d3 | 312 | attitude_get(); |
Khanchana | 0:43d21d5145d3 | 313 | |
Khanchana | 0:43d21d5145d3 | 314 | //pc.printf(" %f \t", ax*10 ); |
Khanchana | 0:43d21d5145d3 | 315 | //pc.printf(" %f \t", ay*10 ); |
Khanchana | 0:43d21d5145d3 | 316 | //pc.printf(" %f \t", az*10-10); //cm/s*s |
Khanchana | 0:43d21d5145d3 | 317 | |
Khanchana | 1:13164a15fbf6 | 318 | //pc.printf("%f\t %.0f \t %.0f \n\r", roll, pitch, yaw); |
Khanchana | 1:13164a15fbf6 | 319 | |
Khanchana | 1:13164a15fbf6 | 320 | //pc.printf("up\t"); |
Khanchana | 1:13164a15fbf6 | 321 | //pc.printf("%f \t",up_degree); |
Khanchana | 1:13164a15fbf6 | 322 | //pc.printf("%f\t",Cg); |
Khanchana | 1:13164a15fbf6 | 323 | //pc.printf("down\t"); |
Khanchana | 1:13164a15fbf6 | 324 | //pc.printf("%f \t",down_degree); |
Khanchana | 1:13164a15fbf6 | 325 | //pc.printf("%f\n",Cg); |
Khanchana | 0:43d21d5145d3 | 326 | |
Khanchana | 0:43d21d5145d3 | 327 | timer1.reset(); // reset timer |
Khanchana | 0:43d21d5145d3 | 328 | Cal_si(); |
Khanchana | 0:43d21d5145d3 | 329 | } |
Khanchana | 0:43d21d5145d3 | 330 | } |
Khanchana | 0:43d21d5145d3 | 331 | } |
Khanchana | 0:43d21d5145d3 | 332 | |
Khanchana | 0:43d21d5145d3 | 333 | void Cal_si(){ |
Khanchana | 0:43d21d5145d3 | 334 | if(state_count_left == 4 or state_count_right == 4){ |
Khanchana | 0:43d21d5145d3 | 335 | roll_data[i] = roll; |
Khanchana | 1:13164a15fbf6 | 336 | //pc.printf("%f\n",roll_data[i]); |
Khanchana | 0:43d21d5145d3 | 337 | Avg(); |
Khanchana | 0:43d21d5145d3 | 338 | //pc.printf("Avg "); |
Khanchana | 0:43d21d5145d3 | 339 | //pc.printf("%f\n",avg); |
Khanchana | 1:13164a15fbf6 | 340 | Cal_Cg_linear(); |
Khanchana | 1:13164a15fbf6 | 341 | //pc.printf("roll\t"); |
Khanchana | 1:13164a15fbf6 | 342 | pc.printf("%f\t",roll_data[i]); |
Khanchana | 1:13164a15fbf6 | 343 | //pc.printf("Si\t"); |
Khanchana | 1:13164a15fbf6 | 344 | pc.printf("%f\t",Si); |
Khanchana | 1:13164a15fbf6 | 345 | //pc.printf("Cg\t"); |
Khanchana | 1:13164a15fbf6 | 346 | pc.printf("%f\t",Cg); |
Khanchana | 1:13164a15fbf6 | 347 | //pc.printf("down\t"); |
Khanchana | 1:13164a15fbf6 | 348 | pc.printf("%f \t",down_degree); |
Khanchana | 1:13164a15fbf6 | 349 | //pc.printf("up\t"); |
Khanchana | 1:13164a15fbf6 | 350 | pc.printf("%f \n",up_degree); |
Khanchana | 0:43d21d5145d3 | 351 | if(i == 9){ |
Khanchana | 0:43d21d5145d3 | 352 | i = 0; |
Khanchana | 0:43d21d5145d3 | 353 | }else{ |
Khanchana | 0:43d21d5145d3 | 354 | i = i+1; |
Khanchana | 0:43d21d5145d3 | 355 | } |
Khanchana | 0:43d21d5145d3 | 356 | } |
Khanchana | 0:43d21d5145d3 | 357 | } |
Khanchana | 0:43d21d5145d3 | 358 | |
Khanchana | 0:43d21d5145d3 | 359 | void Avg(){ |
Khanchana | 0:43d21d5145d3 | 360 | sum = sum + roll; |
Khanchana | 0:43d21d5145d3 | 361 | avg = sum/10; |
Khanchana | 1:13164a15fbf6 | 362 | standardDeviation = (roll - avg)*(roll-avg); |
Khanchana | 1:13164a15fbf6 | 363 | sd = sqrt(standardDeviation/10); |
Khanchana | 0:43d21d5145d3 | 364 | if(avg < 0){ |
Khanchana | 0:43d21d5145d3 | 365 | avg = avg*(-1); |
Khanchana | 0:43d21d5145d3 | 366 | } |
Khanchana | 0:43d21d5145d3 | 367 | } |
Khanchana | 0:43d21d5145d3 | 368 | |
Khanchana | 1:13164a15fbf6 | 369 | |
Khanchana | 1:13164a15fbf6 | 370 | |
Khanchana | 1:13164a15fbf6 | 371 | void Cal_Cg_linear(){ |
Khanchana | 1:13164a15fbf6 | 372 | Si = sd/0.6; |
Khanchana | 1:13164a15fbf6 | 373 | sum = sum - roll_data[i]; |
Khanchana | 0:43d21d5145d3 | 374 | if(Si > 0){ |
Khanchana | 1:13164a15fbf6 | 375 | Cg = (0.8*Si)+(0.5*Cg); |
Khanchana | 0:43d21d5145d3 | 376 | if(Cg > 1){ |
Khanchana | 0:43d21d5145d3 | 377 | Cg = 1; |
Khanchana | 0:43d21d5145d3 | 378 | } |
Khanchana | 0:43d21d5145d3 | 379 | }else{ |
Khanchana | 0:43d21d5145d3 | 380 | Cg = 0.00; |
Khanchana | 0:43d21d5145d3 | 381 | } |
Khanchana | 1:13164a15fbf6 | 382 | Cg_down = 1/0.1*exp(2*(Cg-2.081))-0.15; |
Khanchana | 1:13164a15fbf6 | 383 | Cg_up = (0.8*log(Cg+0.4))+0.734; |
Khanchana | 0:43d21d5145d3 | 384 | } |