turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
main.cpp@2:18835f8732ad, 2018-06-20 (annotated)
- Committer:
- worasuchad
- Date:
- Wed Jun 20 04:59:45 2018 +0000
- Revision:
- 2:18835f8732ad
- Parent:
- 1:13164a15fbf6
- Child:
- 3:5e867483469e
Modify IMU thread ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
worasuchad | 2:18835f8732ad | 1 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 2 | // project: TurtleBot Project // |
worasuchad | 2:18835f8732ad | 3 | // code v.: 0.0 // |
worasuchad | 2:18835f8732ad | 4 | // board : NUCLEO-F303KB // |
worasuchad | 2:18835f8732ad | 5 | // date : 19/6/2018 // |
worasuchad | 2:18835f8732ad | 6 | // code by: Worasuchad Haomachai // |
worasuchad | 2:18835f8732ad | 7 | // detail : Modify IMU thread // |
worasuchad | 2:18835f8732ad | 8 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 9 | |
worasuchad | 2:18835f8732ad | 10 | ///////////////////////// init //////////////////////////////// |
worasuchad | 2:18835f8732ad | 11 | ////////////////////////////////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 12 | #include "mbed.h" |
Khanchana | 0:43d21d5145d3 | 13 | #include "Servo.h" |
Khanchana | 0:43d21d5145d3 | 14 | #include "rtos.h" |
Khanchana | 0:43d21d5145d3 | 15 | #include "attitude.h" |
Khanchana | 1:13164a15fbf6 | 16 | #include "math.h" |
Khanchana | 0:43d21d5145d3 | 17 | |
worasuchad | 2:18835f8732ad | 18 | Serial pc(USBTX, USBRX); //Serial Port |
Khanchana | 0:43d21d5145d3 | 19 | |
Khanchana | 0:43d21d5145d3 | 20 | Timer timer1; |
Khanchana | 0:43d21d5145d3 | 21 | Timer timerwalk; |
Khanchana | 0:43d21d5145d3 | 22 | Thread thread1; |
Khanchana | 0:43d21d5145d3 | 23 | Thread thread2; |
Khanchana | 0:43d21d5145d3 | 24 | |
Khanchana | 0:43d21d5145d3 | 25 | Servo Servo1(D6); |
Khanchana | 0:43d21d5145d3 | 26 | Servo Servo2(D8); |
Khanchana | 0:43d21d5145d3 | 27 | Servo Servo3(D9); |
Khanchana | 0:43d21d5145d3 | 28 | Servo Servo4(D10); |
Khanchana | 0:43d21d5145d3 | 29 | |
worasuchad | 2:18835f8732ad | 30 | ///////////////////////// prototype func /////////////////////// |
worasuchad | 2:18835f8732ad | 31 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 32 | void servo(); |
Khanchana | 0:43d21d5145d3 | 33 | void IMU(); |
worasuchad | 2:18835f8732ad | 34 | void move(); |
worasuchad | 2:18835f8732ad | 35 | /////// In Move ////// |
worasuchad | 2:18835f8732ad | 36 | void cal_step_down(); |
worasuchad | 2:18835f8732ad | 37 | void cal_step_up(); |
worasuchad | 2:18835f8732ad | 38 | void servo_Right(); |
worasuchad | 2:18835f8732ad | 39 | ////////////////////// |
Khanchana | 0:43d21d5145d3 | 40 | void Cal_si(); |
Khanchana | 0:43d21d5145d3 | 41 | void Avg(); |
Khanchana | 1:13164a15fbf6 | 42 | void Cal_Cg_linear(); |
Khanchana | 1:13164a15fbf6 | 43 | void Cal_Cg_log(); |
Khanchana | 0:43d21d5145d3 | 44 | void receive_hormone(); |
Khanchana | 0:43d21d5145d3 | 45 | |
worasuchad | 2:18835f8732ad | 46 | float calculateSD(float data[]); |
worasuchad | 2:18835f8732ad | 47 | float HormoneCon(float SiPreProcess); |
worasuchad | 2:18835f8732ad | 48 | |
worasuchad | 2:18835f8732ad | 49 | ///////////////////////// variable /////////////////////////////// |
worasuchad | 2:18835f8732ad | 50 | ////////////////////////////////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 51 | int walking_time; |
Khanchana | 0:43d21d5145d3 | 52 | int i = 0; |
Khanchana | 0:43d21d5145d3 | 53 | |
Khanchana | 1:13164a15fbf6 | 54 | float Cg_down = 0.00; |
Khanchana | 1:13164a15fbf6 | 55 | float Cg_up = 0; |
worasuchad | 2:18835f8732ad | 56 | float Cg = 0.00, CgPrevious = 0.00; |
Khanchana | 0:43d21d5145d3 | 57 | |
Khanchana | 0:43d21d5145d3 | 58 | float pos_down_start = 1400.00; |
Khanchana | 0:43d21d5145d3 | 59 | float pos_up_start = 1000.00; |
Khanchana | 0:43d21d5145d3 | 60 | float down_degree = 90.00; |
Khanchana | 0:43d21d5145d3 | 61 | float up_degree = 45.00; |
Khanchana | 0:43d21d5145d3 | 62 | float stepmin = 1; |
worasuchad | 2:18835f8732ad | 63 | float round = 10; |
Khanchana | 0:43d21d5145d3 | 64 | float waittime = 0.001 ; |
Khanchana | 0:43d21d5145d3 | 65 | |
Khanchana | 0:43d21d5145d3 | 66 | float pos_down_left = 1400.00; |
Khanchana | 0:43d21d5145d3 | 67 | float pos_up_left = 1000.00; |
Khanchana | 0:43d21d5145d3 | 68 | float pos_down_end_left; |
Khanchana | 0:43d21d5145d3 | 69 | float pos_up_end_left; |
Khanchana | 0:43d21d5145d3 | 70 | float state_count_left = 1; |
Khanchana | 0:43d21d5145d3 | 71 | float round_count_left = 1; |
Khanchana | 0:43d21d5145d3 | 72 | float step_down_left; |
Khanchana | 0:43d21d5145d3 | 73 | float step_up_left; |
Khanchana | 0:43d21d5145d3 | 74 | |
Khanchana | 0:43d21d5145d3 | 75 | float pos_down_right = 1400.00; |
Khanchana | 0:43d21d5145d3 | 76 | float pos_up_right = 1000.00; |
Khanchana | 0:43d21d5145d3 | 77 | float pos_down_end_right; |
Khanchana | 0:43d21d5145d3 | 78 | float pos_up_end_right; |
Khanchana | 0:43d21d5145d3 | 79 | float state_count_right = 1; |
Khanchana | 0:43d21d5145d3 | 80 | float round_count_right = 1; |
Khanchana | 0:43d21d5145d3 | 81 | float step_up_right; |
Khanchana | 0:43d21d5145d3 | 82 | float step_down_right; |
Khanchana | 0:43d21d5145d3 | 83 | |
worasuchad | 2:18835f8732ad | 84 | ///////////////////////// main //////////////////////////// |
worasuchad | 2:18835f8732ad | 85 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 86 | int main() |
worasuchad | 2:18835f8732ad | 87 | { |
worasuchad | 2:18835f8732ad | 88 | pc.baud(115200); |
worasuchad | 2:18835f8732ad | 89 | timer1.start(); // start timer counting |
worasuchad | 2:18835f8732ad | 90 | pc.printf("PRESS '1'\n"); |
worasuchad | 2:18835f8732ad | 91 | if (pc.getc() == '1') |
worasuchad | 2:18835f8732ad | 92 | { |
worasuchad | 2:18835f8732ad | 93 | thread2.start(servo); // servo thread start |
worasuchad | 2:18835f8732ad | 94 | } |
worasuchad | 2:18835f8732ad | 95 | //thread2.start(servo); |
worasuchad | 2:18835f8732ad | 96 | } |
worasuchad | 2:18835f8732ad | 97 | |
worasuchad | 2:18835f8732ad | 98 | ///////////////////////// Servo /////////////////////////// |
worasuchad | 2:18835f8732ad | 99 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 100 | void servo() |
worasuchad | 2:18835f8732ad | 101 | { |
Khanchana | 0:43d21d5145d3 | 102 | pc.printf("start\n"); |
worasuchad | 2:18835f8732ad | 103 | attitude_setup(); // IMU setup |
worasuchad | 2:18835f8732ad | 104 | thread1.start(IMU); // IMU thread start |
Khanchana | 0:43d21d5145d3 | 105 | //wait(0.01); |
worasuchad | 2:18835f8732ad | 106 | timerwalk.start(); // start timer counting |
worasuchad | 2:18835f8732ad | 107 | move(); // func of driving servo |
Khanchana | 0:43d21d5145d3 | 108 | //pc.printf("%f\n",roll_min); |
Khanchana | 0:43d21d5145d3 | 109 | //pc.printf("%f\n",roll_max); |
Khanchana | 0:43d21d5145d3 | 110 | } |
Khanchana | 0:43d21d5145d3 | 111 | |
worasuchad | 2:18835f8732ad | 112 | ///////////////////////// receive_hormone ///////////////// |
worasuchad | 2:18835f8732ad | 113 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 114 | void receive_hormone() |
worasuchad | 2:18835f8732ad | 115 | { |
worasuchad | 2:18835f8732ad | 116 | //down_degree = 90.00f*(1.00f-(0.06f*Cg_down)); |
worasuchad | 2:18835f8732ad | 117 | down_degree = 83.50; |
worasuchad | 2:18835f8732ad | 118 | /*pc.printf("down\t"); |
Khanchana | 1:13164a15fbf6 | 119 | pc.printf("%f \t",down_degree); |
Khanchana | 1:13164a15fbf6 | 120 | pc.printf("%f\t",Cg);*/ |
worasuchad | 2:18835f8732ad | 121 | //up_degree = 45.00f*(1.00f+(0.7f*Cg_up)); |
worasuchad | 2:18835f8732ad | 122 | up_degree = 45.00; |
Khanchana | 1:13164a15fbf6 | 123 | /*pc.printf("up\t"); |
Khanchana | 1:13164a15fbf6 | 124 | pc.printf("%f \t",up_degree); |
worasuchad | 2:18835f8732ad | 125 | pc.printf("%f\n",Cg); |
worasuchad | 2:18835f8732ad | 126 | if(down_degree < 85) |
worasuchad | 2:18835f8732ad | 127 | { |
Khanchana | 1:13164a15fbf6 | 128 | down_degree = 85; |
worasuchad | 2:18835f8732ad | 129 | if(up_degree > 75) |
worasuchad | 2:18835f8732ad | 130 | { |
worasuchad | 2:18835f8732ad | 131 | up_degree = 75; |
Khanchana | 0:43d21d5145d3 | 132 | } |
worasuchad | 2:18835f8732ad | 133 | }*/ |
Khanchana | 0:43d21d5145d3 | 134 | } |
Khanchana | 0:43d21d5145d3 | 135 | |
worasuchad | 2:18835f8732ad | 136 | ///////////////////////// cal_step_down /////////////////// |
worasuchad | 2:18835f8732ad | 137 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 138 | void cal_step_down() |
worasuchad | 2:18835f8732ad | 139 | { |
Khanchana | 0:43d21d5145d3 | 140 | //pc.printf("down"); |
Khanchana | 0:43d21d5145d3 | 141 | //pc.printf("%f \n",down_degree); |
worasuchad | 2:18835f8732ad | 142 | pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); // get degree for hormone receiver about down_degree ~ 90*, |
worasuchad | 2:18835f8732ad | 143 | pos_down_end_right = (1070.00 + ((700.00/90.00)*(down_degree))); // so both pos_down_end_left and pos_down_end_right are around 1700 |
worasuchad | 2:18835f8732ad | 144 | if (pos_down_end_right > pos_down_end_left) |
worasuchad | 2:18835f8732ad | 145 | { |
worasuchad | 2:18835f8732ad | 146 | step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start); //stepmin = 1, pos_down_start = 1400.00 |
Khanchana | 0:43d21d5145d3 | 147 | step_down_left = stepmin; |
worasuchad | 2:18835f8732ad | 148 | } |
worasuchad | 2:18835f8732ad | 149 | else if (pos_down_end_right < pos_down_end_left) |
worasuchad | 2:18835f8732ad | 150 | { |
Khanchana | 0:43d21d5145d3 | 151 | step_down_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 152 | step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start); |
worasuchad | 2:18835f8732ad | 153 | } |
worasuchad | 2:18835f8732ad | 154 | else // pos_down_end_right == pos_down_end_left |
worasuchad | 2:18835f8732ad | 155 | { |
Khanchana | 0:43d21d5145d3 | 156 | step_down_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 157 | step_down_left = stepmin; |
Khanchana | 0:43d21d5145d3 | 158 | } |
Khanchana | 0:43d21d5145d3 | 159 | /*pc.printf("pos_down_right"); |
Khanchana | 0:43d21d5145d3 | 160 | pc.printf("%f\n",pos_down_end_right); |
Khanchana | 0:43d21d5145d3 | 161 | pc.printf("pos_down_left"); |
Khanchana | 0:43d21d5145d3 | 162 | pc.printf("%f\n",pos_down_end_left); |
Khanchana | 0:43d21d5145d3 | 163 | pc.printf("step_down_right"); |
Khanchana | 0:43d21d5145d3 | 164 | pc.printf("%f\n",step_down_right); |
Khanchana | 0:43d21d5145d3 | 165 | pc.printf("step_down_left"); |
Khanchana | 0:43d21d5145d3 | 166 | pc.printf("%f\n",step_down_left);*/ |
Khanchana | 0:43d21d5145d3 | 167 | } |
worasuchad | 2:18835f8732ad | 168 | |
worasuchad | 2:18835f8732ad | 169 | ///////////////////////// cal_step_up ///////////////////// |
worasuchad | 2:18835f8732ad | 170 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 171 | void cal_step_up() |
worasuchad | 2:18835f8732ad | 172 | { |
Khanchana | 0:43d21d5145d3 | 173 | //pc.printf("up"); |
Khanchana | 0:43d21d5145d3 | 174 | //pc.printf("%f \n",up_degree); |
worasuchad | 2:18835f8732ad | 175 | pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); // get degree for hormone receiver about up_degree ~ 45*, |
worasuchad | 2:18835f8732ad | 176 | pos_up_end_right = 1000.00 + ((700.00/90.00)*(up_degree)); // so both pos_up_end_left and pos_up_end_right are around 1350 |
worasuchad | 2:18835f8732ad | 177 | if (pos_up_end_right > pos_up_end_left) |
worasuchad | 2:18835f8732ad | 178 | { |
worasuchad | 2:18835f8732ad | 179 | step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start); //stepmin = 1, pos_up_start = 1000.00 |
Khanchana | 0:43d21d5145d3 | 180 | step_up_left = stepmin; |
worasuchad | 2:18835f8732ad | 181 | } |
worasuchad | 2:18835f8732ad | 182 | else if (pos_up_end_right < pos_up_end_left) |
worasuchad | 2:18835f8732ad | 183 | { |
Khanchana | 0:43d21d5145d3 | 184 | step_up_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 185 | step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start); |
worasuchad | 2:18835f8732ad | 186 | } |
worasuchad | 2:18835f8732ad | 187 | else // step_up_right == step_up_left |
worasuchad | 2:18835f8732ad | 188 | { |
Khanchana | 0:43d21d5145d3 | 189 | step_up_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 190 | step_up_left = stepmin; |
Khanchana | 0:43d21d5145d3 | 191 | } |
Khanchana | 0:43d21d5145d3 | 192 | /*pc.printf("pos_up_right"); |
Khanchana | 0:43d21d5145d3 | 193 | pc.printf("%f\n",pos_up_end_right); |
Khanchana | 0:43d21d5145d3 | 194 | pc.printf("pos_up_left"); |
Khanchana | 0:43d21d5145d3 | 195 | pc.printf("%f\n",pos_up_end_left); |
Khanchana | 0:43d21d5145d3 | 196 | pc.printf("step_up_right"); |
Khanchana | 0:43d21d5145d3 | 197 | pc.printf("%f\n",step_up_right);; |
Khanchana | 0:43d21d5145d3 | 198 | pc.printf("step_up_left"); |
worasuchad | 2:18835f8732ad | 199 | pc.printf("%f\n",step_up_left);*/ |
Khanchana | 0:43d21d5145d3 | 200 | } |
worasuchad | 2:18835f8732ad | 201 | |
worasuchad | 2:18835f8732ad | 202 | ///////////////////////// move //////////////////////////// |
worasuchad | 2:18835f8732ad | 203 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 204 | void move() |
worasuchad | 2:18835f8732ad | 205 | { |
Khanchana | 1:13164a15fbf6 | 206 | Servo1.Enable(1000,20000); |
Khanchana | 1:13164a15fbf6 | 207 | Servo2.Enable(1000,20000); |
Khanchana | 1:13164a15fbf6 | 208 | Servo3.Enable(1000,20000); |
worasuchad | 2:18835f8732ad | 209 | Servo4.Enable(1000,20000); |
Khanchana | 1:13164a15fbf6 | 210 | /*pc.printf("Si\t"); |
Khanchana | 1:13164a15fbf6 | 211 | pc.printf("%f \t",Si); |
Khanchana | 1:13164a15fbf6 | 212 | pc.printf("down\t"); |
Khanchana | 1:13164a15fbf6 | 213 | pc.printf("%f \t",down_degree); |
Khanchana | 1:13164a15fbf6 | 214 | pc.printf("%f\t",Cg); |
Khanchana | 1:13164a15fbf6 | 215 | pc.printf("up\t"); |
Khanchana | 1:13164a15fbf6 | 216 | pc.printf("%f \t",up_degree); |
Khanchana | 1:13164a15fbf6 | 217 | pc.printf("%f\n",Cg);*/ |
worasuchad | 2:18835f8732ad | 218 | while(1) |
worasuchad | 2:18835f8732ad | 219 | { |
worasuchad | 2:18835f8732ad | 220 | cal_step_down(); // return "step_down_right" and "step_down_left" |
worasuchad | 2:18835f8732ad | 221 | cal_step_up(); // return "step_up_right" and "step_up_left" |
worasuchad | 2:18835f8732ad | 222 | servo_Right(); // control right leg |
worasuchad | 2:18835f8732ad | 223 | |
worasuchad | 2:18835f8732ad | 224 | if(state_count_left == 1) // control left lag |
worasuchad | 2:18835f8732ad | 225 | { |
worasuchad | 2:18835f8732ad | 226 | Servo1.SetPosition(pos_down_left); // pos_down_left = 1400.00 |
worasuchad | 2:18835f8732ad | 227 | wait(waittime); // 0.001 ms |
Khanchana | 0:43d21d5145d3 | 228 | pos_down_left = pos_down_left + step_down_left; |
worasuchad | 2:18835f8732ad | 229 | if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) // pos_down_end_left ~ 1700 |
worasuchad | 2:18835f8732ad | 230 | { |
Khanchana | 0:43d21d5145d3 | 231 | state_count_left = 2; |
Khanchana | 0:43d21d5145d3 | 232 | } |
Khanchana | 0:43d21d5145d3 | 233 | /*pc.printf("LAD"); |
Khanchana | 0:43d21d5145d3 | 234 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 235 | pc.printf("LAP"); |
Khanchana | 0:43d21d5145d3 | 236 | pc.printf("%f\n",pos_up_left);*/ |
worasuchad | 2:18835f8732ad | 237 | } |
worasuchad | 2:18835f8732ad | 238 | else if(state_count_left == 2) |
worasuchad | 2:18835f8732ad | 239 | { |
worasuchad | 2:18835f8732ad | 240 | Servo2.SetPosition(pos_up_left); // pos_up_left = 1000.00 |
Khanchana | 0:43d21d5145d3 | 241 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 242 | pos_up_left = pos_up_left + step_up_left; |
worasuchad | 2:18835f8732ad | 243 | if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) |
worasuchad | 2:18835f8732ad | 244 | { |
Khanchana | 0:43d21d5145d3 | 245 | state_count_left = 3; |
Khanchana | 0:43d21d5145d3 | 246 | } |
Khanchana | 0:43d21d5145d3 | 247 | /*pc.printf("LBD"); |
Khanchana | 0:43d21d5145d3 | 248 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 249 | pc.printf("LBP"); |
Khanchana | 0:43d21d5145d3 | 250 | pc.printf("%f\n",pos_up_left);*/ |
worasuchad | 2:18835f8732ad | 251 | } |
worasuchad | 2:18835f8732ad | 252 | else if(state_count_left == 3) |
worasuchad | 2:18835f8732ad | 253 | { |
Khanchana | 0:43d21d5145d3 | 254 | Servo1.SetPosition(pos_down_left); |
Khanchana | 0:43d21d5145d3 | 255 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 256 | pos_down_left = pos_down_left - step_down_left; |
worasuchad | 2:18835f8732ad | 257 | if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) |
worasuchad | 2:18835f8732ad | 258 | { |
Khanchana | 0:43d21d5145d3 | 259 | state_count_left = 4; |
Khanchana | 0:43d21d5145d3 | 260 | } |
Khanchana | 0:43d21d5145d3 | 261 | /*pc.printf("LCD"); |
Khanchana | 0:43d21d5145d3 | 262 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 263 | pc.printf("LCP"); |
Khanchana | 0:43d21d5145d3 | 264 | pc.printf("%f\n",pos_up_left);*/ |
worasuchad | 2:18835f8732ad | 265 | } |
worasuchad | 2:18835f8732ad | 266 | else if(state_count_left == 4) |
worasuchad | 2:18835f8732ad | 267 | { |
Khanchana | 0:43d21d5145d3 | 268 | Servo2.SetPosition(pos_up_left); |
Khanchana | 0:43d21d5145d3 | 269 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 270 | pos_up_left = pos_up_left - step_up_left; |
worasuchad | 2:18835f8732ad | 271 | if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) |
worasuchad | 2:18835f8732ad | 272 | { |
Khanchana | 0:43d21d5145d3 | 273 | state_count_left = 0; |
Khanchana | 0:43d21d5145d3 | 274 | } |
Khanchana | 0:43d21d5145d3 | 275 | /*pc.printf("LDD"); |
Khanchana | 0:43d21d5145d3 | 276 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 277 | pc.printf("LDP"); |
Khanchana | 0:43d21d5145d3 | 278 | pc.printf("%f\n",pos_up_left);*/ |
worasuchad | 2:18835f8732ad | 279 | } |
worasuchad | 2:18835f8732ad | 280 | else if (state_count_left == 0 and round_count_left < round) |
worasuchad | 2:18835f8732ad | 281 | { |
Khanchana | 0:43d21d5145d3 | 282 | round_count_left = round_count_left+1; |
Khanchana | 0:43d21d5145d3 | 283 | state_count_left = 1; |
Khanchana | 0:43d21d5145d3 | 284 | pos_down_left = pos_down_start; |
Khanchana | 0:43d21d5145d3 | 285 | pos_up_left = pos_up_start; |
worasuchad | 2:18835f8732ad | 286 | } |
worasuchad | 2:18835f8732ad | 287 | else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round) |
worasuchad | 2:18835f8732ad | 288 | { |
Khanchana | 0:43d21d5145d3 | 289 | thread1.terminate(); |
worasuchad | 2:18835f8732ad | 290 | //pc.printf("Finish \n"); |
Khanchana | 0:43d21d5145d3 | 291 | walking_time = timerwalk.read_ms(); |
worasuchad | 2:18835f8732ad | 292 | //pc.printf("Walking time = %d \n", walking_time); |
Khanchana | 0:43d21d5145d3 | 293 | break; |
Khanchana | 0:43d21d5145d3 | 294 | } |
Khanchana | 0:43d21d5145d3 | 295 | } |
Khanchana | 0:43d21d5145d3 | 296 | } |
worasuchad | 2:18835f8732ad | 297 | |
worasuchad | 2:18835f8732ad | 298 | ///////////////////////// servo_Right ///////////////////// |
worasuchad | 2:18835f8732ad | 299 | ////////////////////////////////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 300 | void servo_Right() |
Khanchana | 0:43d21d5145d3 | 301 | { |
worasuchad | 2:18835f8732ad | 302 | if(state_count_right == 1) |
worasuchad | 2:18835f8732ad | 303 | { |
Khanchana | 0:43d21d5145d3 | 304 | Servo3.SetPosition(pos_down_right); |
Khanchana | 0:43d21d5145d3 | 305 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 306 | pos_down_right = pos_down_right + step_down_right; |
worasuchad | 2:18835f8732ad | 307 | if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) |
worasuchad | 2:18835f8732ad | 308 | { |
Khanchana | 0:43d21d5145d3 | 309 | state_count_right = 2; |
Khanchana | 0:43d21d5145d3 | 310 | } |
Khanchana | 0:43d21d5145d3 | 311 | /*pc.printf("RAD"); |
Khanchana | 0:43d21d5145d3 | 312 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 313 | pc.printf("RAP"); |
Khanchana | 0:43d21d5145d3 | 314 | pc.printf("%f\n",pos_up_right);*/ |
worasuchad | 2:18835f8732ad | 315 | } |
worasuchad | 2:18835f8732ad | 316 | else if(state_count_right == 2) |
worasuchad | 2:18835f8732ad | 317 | { |
Khanchana | 0:43d21d5145d3 | 318 | Servo4.SetPosition(pos_up_right); |
Khanchana | 0:43d21d5145d3 | 319 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 320 | pos_up_right = pos_up_right + step_up_right; |
worasuchad | 2:18835f8732ad | 321 | if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) |
worasuchad | 2:18835f8732ad | 322 | { |
Khanchana | 0:43d21d5145d3 | 323 | state_count_right = 3; |
Khanchana | 0:43d21d5145d3 | 324 | } |
Khanchana | 0:43d21d5145d3 | 325 | /*pc.printf("RBD"); |
Khanchana | 0:43d21d5145d3 | 326 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 327 | pc.printf("RBP"); |
Khanchana | 0:43d21d5145d3 | 328 | pc.printf("%f\n",pos_up_right);*/ |
worasuchad | 2:18835f8732ad | 329 | } |
worasuchad | 2:18835f8732ad | 330 | else if(state_count_right == 3) |
worasuchad | 2:18835f8732ad | 331 | { |
Khanchana | 0:43d21d5145d3 | 332 | Servo3.SetPosition(pos_down_right); |
Khanchana | 0:43d21d5145d3 | 333 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 334 | pos_down_right = pos_down_right - step_down_right; |
worasuchad | 2:18835f8732ad | 335 | if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) |
worasuchad | 2:18835f8732ad | 336 | { |
Khanchana | 0:43d21d5145d3 | 337 | state_count_right = 4; |
Khanchana | 0:43d21d5145d3 | 338 | } |
Khanchana | 0:43d21d5145d3 | 339 | /*pc.printf("RCD"); |
Khanchana | 0:43d21d5145d3 | 340 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 341 | pc.printf("RCP"); |
Khanchana | 0:43d21d5145d3 | 342 | pc.printf("%f\n",pos_up_right);*/ |
worasuchad | 2:18835f8732ad | 343 | } |
worasuchad | 2:18835f8732ad | 344 | else if(state_count_right == 4) |
worasuchad | 2:18835f8732ad | 345 | { |
Khanchana | 0:43d21d5145d3 | 346 | Servo4.SetPosition(pos_up_right); |
worasuchad | 2:18835f8732ad | 347 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 348 | pos_up_right = pos_up_right - step_up_right; |
worasuchad | 2:18835f8732ad | 349 | if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) |
worasuchad | 2:18835f8732ad | 350 | { |
Khanchana | 0:43d21d5145d3 | 351 | state_count_right = 0; |
Khanchana | 0:43d21d5145d3 | 352 | } |
Khanchana | 0:43d21d5145d3 | 353 | /*pc.printf("RDD"); |
Khanchana | 0:43d21d5145d3 | 354 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 355 | pc.printf("RDP"); |
Khanchana | 0:43d21d5145d3 | 356 | pc.printf("%f\n",pos_up_right);*/ |
worasuchad | 2:18835f8732ad | 357 | } |
worasuchad | 2:18835f8732ad | 358 | else if (state_count_right == 0 and round_count_right < round) |
worasuchad | 2:18835f8732ad | 359 | { |
Khanchana | 0:43d21d5145d3 | 360 | round_count_right = round_count_right+1; |
Khanchana | 0:43d21d5145d3 | 361 | state_count_right = 1; |
Khanchana | 0:43d21d5145d3 | 362 | pos_down_right = pos_down_start; |
Khanchana | 0:43d21d5145d3 | 363 | pos_up_right = pos_up_start; |
Khanchana | 0:43d21d5145d3 | 364 | receive_hormone(); |
Khanchana | 0:43d21d5145d3 | 365 | } |
Khanchana | 0:43d21d5145d3 | 366 | } |
Khanchana | 0:43d21d5145d3 | 367 | |
worasuchad | 2:18835f8732ad | 368 | |
worasuchad | 2:18835f8732ad | 369 | ///////////////////////// IMU ///////////////////////////// |
worasuchad | 2:18835f8732ad | 370 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 371 | void IMU() |
worasuchad | 2:18835f8732ad | 372 | { |
worasuchad | 2:18835f8732ad | 373 | int i; |
worasuchad | 2:18835f8732ad | 374 | float SiInput[10]; |
worasuchad | 2:18835f8732ad | 375 | float SDOfRoll; |
worasuchad | 2:18835f8732ad | 376 | /* pc.printf("roll\t"); |
Khanchana | 1:13164a15fbf6 | 377 | pc.printf("Si\t"); |
Khanchana | 1:13164a15fbf6 | 378 | pc.printf("Cg\t"); |
Khanchana | 1:13164a15fbf6 | 379 | pc.printf("down\t"); |
worasuchad | 2:18835f8732ad | 380 | pc.printf("up\n"); */ |
worasuchad | 2:18835f8732ad | 381 | while(1) |
worasuchad | 2:18835f8732ad | 382 | { |
worasuchad | 2:18835f8732ad | 383 | if (timer1.read_us() >= 1000) // read time in 1 ms |
Khanchana | 0:43d21d5145d3 | 384 | { |
Khanchana | 0:43d21d5145d3 | 385 | attitude_get(); |
Khanchana | 0:43d21d5145d3 | 386 | |
Khanchana | 0:43d21d5145d3 | 387 | //pc.printf(" %f \t", ax*10 ); |
Khanchana | 0:43d21d5145d3 | 388 | //pc.printf(" %f \t", ay*10 ); |
Khanchana | 0:43d21d5145d3 | 389 | //pc.printf(" %f \t", az*10-10); //cm/s*s |
Khanchana | 0:43d21d5145d3 | 390 | |
worasuchad | 2:18835f8732ad | 391 | pc.printf("%f\t %f\t %f\n\r", roll, pitch, yaw); |
Khanchana | 1:13164a15fbf6 | 392 | //pc.printf("up\t"); |
Khanchana | 1:13164a15fbf6 | 393 | //pc.printf("%f \t",up_degree); |
Khanchana | 1:13164a15fbf6 | 394 | //pc.printf("%f\t",Cg); |
Khanchana | 1:13164a15fbf6 | 395 | //pc.printf("down\t"); |
Khanchana | 1:13164a15fbf6 | 396 | //pc.printf("%f \t",down_degree); |
Khanchana | 1:13164a15fbf6 | 397 | //pc.printf("%f\n",Cg); |
worasuchad | 2:18835f8732ad | 398 | |
worasuchad | 2:18835f8732ad | 399 | ////////////////////////// Signal Pre-Process every 10 ms ////////////////////// |
worasuchad | 2:18835f8732ad | 400 | //////////////////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 401 | if(i < 10) |
worasuchad | 2:18835f8732ad | 402 | { |
worasuchad | 2:18835f8732ad | 403 | SiInput[i] = roll; |
worasuchad | 2:18835f8732ad | 404 | //pc.printf("i = %i ,SiInput = %.2f, roll= %.2f\n\r",i, SiInput[i], roll); |
worasuchad | 2:18835f8732ad | 405 | i++; |
worasuchad | 2:18835f8732ad | 406 | } |
worasuchad | 2:18835f8732ad | 407 | else |
worasuchad | 2:18835f8732ad | 408 | { |
worasuchad | 2:18835f8732ad | 409 | //pc.printf("Roll - Standard Deviation = %.2f \n\r", calculateSD(SiInput)); // every 10 ms |
worasuchad | 2:18835f8732ad | 410 | SDOfRoll = calculateSD(SiInput); |
worasuchad | 2:18835f8732ad | 411 | //pc.printf("HC: %.3f\n\r", HormoneCon(SDOfRoll)); |
worasuchad | 2:18835f8732ad | 412 | |
worasuchad | 2:18835f8732ad | 413 | i = 0; |
worasuchad | 2:18835f8732ad | 414 | } |
worasuchad | 2:18835f8732ad | 415 | |
worasuchad | 2:18835f8732ad | 416 | timer1.reset(); // reset timer |
Khanchana | 0:43d21d5145d3 | 417 | } |
Khanchana | 0:43d21d5145d3 | 418 | } |
Khanchana | 0:43d21d5145d3 | 419 | } |
Khanchana | 0:43d21d5145d3 | 420 | |
worasuchad | 2:18835f8732ad | 421 | ///////////////// Hormone Concentration ///////////////////// |
worasuchad | 2:18835f8732ad | 422 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 423 | float HormoneCon(float SiPreProcess) |
worasuchad | 2:18835f8732ad | 424 | { |
worasuchad | 2:18835f8732ad | 425 | float CgTemp; |
worasuchad | 2:18835f8732ad | 426 | |
worasuchad | 2:18835f8732ad | 427 | //pc.printf("SiPreProcess: %.3f\t", SiPreProcess); |
worasuchad | 2:18835f8732ad | 428 | //pc.printf("CgPrevious: %.3f\t", CgPrevious); |
worasuchad | 2:18835f8732ad | 429 | |
worasuchad | 2:18835f8732ad | 430 | ////// hormone gland ////// |
worasuchad | 2:18835f8732ad | 431 | CgTemp = (0.8f*SiPreProcess) + (0.5f*CgPrevious); |
worasuchad | 2:18835f8732ad | 432 | //pc.printf("CgTemp: %.3f\t", CgTemp); |
worasuchad | 2:18835f8732ad | 433 | Cg = 1/( 1+exp(-CgTemp) ); // used sigmoid func for calculating Cg which much have value between 0 - 1 |
worasuchad | 2:18835f8732ad | 434 | /////////////////////////// |
worasuchad | 2:18835f8732ad | 435 | |
worasuchad | 2:18835f8732ad | 436 | //pc.printf("First Term: %.3f\t", (float)(0.8f*SiPreProcess)); |
worasuchad | 2:18835f8732ad | 437 | //pc.printf("Secound Term: %.3f\t", (float)(0.5f*CgPrevious)); |
worasuchad | 2:18835f8732ad | 438 | CgPrevious = Cg; |
worasuchad | 2:18835f8732ad | 439 | //*********// |
worasuchad | 2:18835f8732ad | 440 | Cg_down = Cg; |
worasuchad | 2:18835f8732ad | 441 | Cg_up = Cg; |
worasuchad | 2:18835f8732ad | 442 | |
worasuchad | 2:18835f8732ad | 443 | return Cg; |
Khanchana | 0:43d21d5145d3 | 444 | } |
Khanchana | 0:43d21d5145d3 | 445 | |
worasuchad | 2:18835f8732ad | 446 | ///////////////////////// calculateSD ///////////////////// |
worasuchad | 2:18835f8732ad | 447 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 448 | float calculateSD(float sdData[]) |
worasuchad | 2:18835f8732ad | 449 | { |
worasuchad | 2:18835f8732ad | 450 | float sum = 0.0, mean, standardDeviation = 0.0; |
worasuchad | 2:18835f8732ad | 451 | int i; |
worasuchad | 2:18835f8732ad | 452 | |
worasuchad | 2:18835f8732ad | 453 | for(i = 0; i < 10 ; i++) |
worasuchad | 2:18835f8732ad | 454 | { |
worasuchad | 2:18835f8732ad | 455 | sum += sdData[i]; |
worasuchad | 2:18835f8732ad | 456 | //pc.printf("sum = %.2f \n\r",sum); |
worasuchad | 2:18835f8732ad | 457 | } |
worasuchad | 2:18835f8732ad | 458 | mean = sum/10; |
worasuchad | 2:18835f8732ad | 459 | //pc.printf("mean = %.2f \n\r",mean); |
worasuchad | 2:18835f8732ad | 460 | |
worasuchad | 2:18835f8732ad | 461 | for(i = 0; i < 10; i++) |
worasuchad | 2:18835f8732ad | 462 | { |
worasuchad | 2:18835f8732ad | 463 | standardDeviation += pow(sdData[i] - mean, 2); |
worasuchad | 2:18835f8732ad | 464 | //pc.printf("standardDeviation = %.2f \n\r",standardDeviation); |
Khanchana | 0:43d21d5145d3 | 465 | } |
worasuchad | 2:18835f8732ad | 466 | return sqrt(standardDeviation / 10); |
Khanchana | 0:43d21d5145d3 | 467 | } |