turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
main.cpp@3:5e867483469e, 2018-06-21 (annotated)
- Committer:
- worasuchad
- Date:
- Thu Jun 21 06:05:00 2018 +0000
- Revision:
- 3:5e867483469e
- Parent:
- 2:18835f8732ad
- Child:
- 4:ec7e68b84f2b
update direction
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 | 3:5e867483469e | 32 | void IMU(); |
worasuchad | 2:18835f8732ad | 33 | void servo(); |
worasuchad | 3:5e867483469e | 34 | /////// In servo ////// |
worasuchad | 2:18835f8732ad | 35 | void cal_step_down(); |
worasuchad | 2:18835f8732ad | 36 | void cal_step_up(); |
worasuchad | 3:5e867483469e | 37 | void servo_Left(); |
worasuchad | 2:18835f8732ad | 38 | void servo_Right(); |
worasuchad | 2:18835f8732ad | 39 | ////////////////////// |
Khanchana | 0:43d21d5145d3 | 40 | void receive_hormone(); |
Khanchana | 0:43d21d5145d3 | 41 | |
worasuchad | 2:18835f8732ad | 42 | float calculateSD(float data[]); |
worasuchad | 3:5e867483469e | 43 | float calculateMean(float meanData[]); |
worasuchad | 2:18835f8732ad | 44 | float HormoneCon(float SiPreProcess); |
worasuchad | 3:5e867483469e | 45 | bool directionRobot(float directionMean, float directionSD); |
worasuchad | 2:18835f8732ad | 46 | |
worasuchad | 2:18835f8732ad | 47 | ///////////////////////// variable /////////////////////////////// |
worasuchad | 2:18835f8732ad | 48 | ////////////////////////////////////////////////////////////////// |
worasuchad | 3:5e867483469e | 49 | // home param |
Khanchana | 0:43d21d5145d3 | 50 | int walking_time; |
worasuchad | 3:5e867483469e | 51 | int initDirection = 0; |
Khanchana | 0:43d21d5145d3 | 52 | |
worasuchad | 3:5e867483469e | 53 | // hormone param |
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; |
worasuchad | 3:5e867483469e | 57 | |
worasuchad | 3:5e867483469e | 58 | // servo motor param |
Khanchana | 0:43d21d5145d3 | 59 | float pos_down_start = 1400.00; |
Khanchana | 0:43d21d5145d3 | 60 | float pos_up_start = 1000.00; |
Khanchana | 0:43d21d5145d3 | 61 | float down_degree = 90.00; |
Khanchana | 0:43d21d5145d3 | 62 | float up_degree = 45.00; |
Khanchana | 0:43d21d5145d3 | 63 | float stepmin = 1; |
worasuchad | 3:5e867483469e | 64 | float round = 5; |
Khanchana | 0:43d21d5145d3 | 65 | float waittime = 0.001 ; |
Khanchana | 0:43d21d5145d3 | 66 | |
Khanchana | 0:43d21d5145d3 | 67 | float pos_down_left = 1400.00; |
Khanchana | 0:43d21d5145d3 | 68 | float pos_up_left = 1000.00; |
Khanchana | 0:43d21d5145d3 | 69 | float pos_down_end_left; |
Khanchana | 0:43d21d5145d3 | 70 | float pos_up_end_left; |
Khanchana | 0:43d21d5145d3 | 71 | float state_count_left = 1; |
Khanchana | 0:43d21d5145d3 | 72 | float round_count_left = 1; |
Khanchana | 0:43d21d5145d3 | 73 | float step_down_left; |
Khanchana | 0:43d21d5145d3 | 74 | float step_up_left; |
Khanchana | 0:43d21d5145d3 | 75 | |
Khanchana | 0:43d21d5145d3 | 76 | float pos_down_right = 1400.00; |
Khanchana | 0:43d21d5145d3 | 77 | float pos_up_right = 1000.00; |
Khanchana | 0:43d21d5145d3 | 78 | float pos_down_end_right; |
Khanchana | 0:43d21d5145d3 | 79 | float pos_up_end_right; |
Khanchana | 0:43d21d5145d3 | 80 | float state_count_right = 1; |
Khanchana | 0:43d21d5145d3 | 81 | float round_count_right = 1; |
Khanchana | 0:43d21d5145d3 | 82 | float step_up_right; |
Khanchana | 0:43d21d5145d3 | 83 | float step_down_right; |
Khanchana | 0:43d21d5145d3 | 84 | |
worasuchad | 2:18835f8732ad | 85 | ///////////////////////// main //////////////////////////// |
worasuchad | 2:18835f8732ad | 86 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 87 | int main() |
worasuchad | 2:18835f8732ad | 88 | { |
worasuchad | 2:18835f8732ad | 89 | pc.baud(115200); |
worasuchad | 3:5e867483469e | 90 | timer1.start(); // start timer counting |
worasuchad | 3:5e867483469e | 91 | attitude_setup(); // IMU setup |
worasuchad | 3:5e867483469e | 92 | //thread1.start(IMU); // IMU thread start |
worasuchad | 3:5e867483469e | 93 | pc.printf(" Please press! '1' to start..\n\r"); |
worasuchad | 2:18835f8732ad | 94 | if (pc.getc() == '1') |
worasuchad | 3:5e867483469e | 95 | { |
worasuchad | 3:5e867483469e | 96 | thread1.start(IMU); // IMU thread start |
worasuchad | 3:5e867483469e | 97 | //thread2.start(servo); // servo thread start |
worasuchad | 3:5e867483469e | 98 | } |
worasuchad | 3:5e867483469e | 99 | } |
worasuchad | 3:5e867483469e | 100 | |
worasuchad | 3:5e867483469e | 101 | ///////////////////////// IMU ///////////////////////////// |
worasuchad | 3:5e867483469e | 102 | ////////////////////////////////////////////////////////////////// |
worasuchad | 3:5e867483469e | 103 | void IMU() |
worasuchad | 3:5e867483469e | 104 | { |
worasuchad | 3:5e867483469e | 105 | int i; |
worasuchad | 3:5e867483469e | 106 | float ArrayOfRoll[10], ArrayOfYaw[10]; |
worasuchad | 3:5e867483469e | 107 | float SDOfRoll, SDOfYaw, MeanOfYaw, directOfRobot = 0.00; |
worasuchad | 3:5e867483469e | 108 | /* pc.printf("roll\t"); |
worasuchad | 3:5e867483469e | 109 | pc.printf("Si\t"); |
worasuchad | 3:5e867483469e | 110 | pc.printf("Cg\t"); |
worasuchad | 3:5e867483469e | 111 | pc.printf("down\t"); |
worasuchad | 3:5e867483469e | 112 | pc.printf("up\n"); */ |
worasuchad | 3:5e867483469e | 113 | while(1) |
worasuchad | 3:5e867483469e | 114 | { |
worasuchad | 3:5e867483469e | 115 | if (timer1.read_us() >= 1000) // read time in 1 ms |
worasuchad | 3:5e867483469e | 116 | { |
worasuchad | 3:5e867483469e | 117 | attitude_get(); |
worasuchad | 3:5e867483469e | 118 | |
worasuchad | 3:5e867483469e | 119 | //pc.printf(" %f \t", ax*10 ); |
worasuchad | 3:5e867483469e | 120 | //pc.printf(" %f \t", ay*10 ); |
worasuchad | 3:5e867483469e | 121 | //pc.printf(" %f \t", az*10-10); //cm/s*s |
worasuchad | 3:5e867483469e | 122 | |
worasuchad | 3:5e867483469e | 123 | //pc.printf("%f\t %f\t %f\n\r", roll, pitch, yaw); |
worasuchad | 3:5e867483469e | 124 | pc.printf("%f\n\r",yaw); |
worasuchad | 3:5e867483469e | 125 | //pc.printf("up\t"); |
worasuchad | 3:5e867483469e | 126 | //pc.printf("%f \t",up_degree); |
worasuchad | 3:5e867483469e | 127 | //pc.printf("%f\t",Cg); |
worasuchad | 3:5e867483469e | 128 | //pc.printf("down\t"); |
worasuchad | 3:5e867483469e | 129 | //pc.printf("%f \t",down_degree); |
worasuchad | 3:5e867483469e | 130 | //pc.printf("%f\n",Cg); |
worasuchad | 3:5e867483469e | 131 | |
worasuchad | 3:5e867483469e | 132 | ////////////////////////// Signal Pre-Process every 10 ms ////////////////////// |
worasuchad | 3:5e867483469e | 133 | //////////////////////////////////////////////////////////////////////////////// |
worasuchad | 3:5e867483469e | 134 | if(i < 10) |
worasuchad | 3:5e867483469e | 135 | { |
worasuchad | 3:5e867483469e | 136 | ArrayOfRoll[i] = roll; |
worasuchad | 3:5e867483469e | 137 | ArrayOfYaw[i] = yaw; |
worasuchad | 3:5e867483469e | 138 | //pc.printf("i = %i ,ArrayOfRoll = %.2f, roll= %.2f\n\r",i, ArrayOfRoll[i], roll); |
worasuchad | 3:5e867483469e | 139 | i++; |
worasuchad | 3:5e867483469e | 140 | } |
worasuchad | 3:5e867483469e | 141 | else |
worasuchad | 3:5e867483469e | 142 | { |
worasuchad | 3:5e867483469e | 143 | //////////// roll ////////////// |
worasuchad | 3:5e867483469e | 144 | //pc.printf("Roll - Standard Deviation = %.2f \n\r", calculateSD(ArrayOfRoll)); // every 10 ms |
worasuchad | 3:5e867483469e | 145 | SDOfRoll = calculateSD(ArrayOfRoll); |
worasuchad | 3:5e867483469e | 146 | //pc.printf("HC: %.3f\n\r", HormoneCon(SDOfRoll)); |
worasuchad | 3:5e867483469e | 147 | HormoneCon(SDOfRoll); |
worasuchad | 3:5e867483469e | 148 | |
worasuchad | 3:5e867483469e | 149 | //////////// yaw /////////////// |
worasuchad | 3:5e867483469e | 150 | MeanOfYaw = calculateMean(ArrayOfYaw); |
worasuchad | 3:5e867483469e | 151 | pc.printf("MeanOfYaw: %.3f\n\r", MeanOfYaw); |
worasuchad | 3:5e867483469e | 152 | SDOfYaw = calculateSD(ArrayOfYaw); |
worasuchad | 3:5e867483469e | 153 | pc.printf("SDOfYaw: %.3f\n\r", SDOfYaw); |
worasuchad | 3:5e867483469e | 154 | |
worasuchad | 3:5e867483469e | 155 | if(directionRobot(MeanOfYaw, SDOfYaw)) // only one time for comming |
worasuchad | 3:5e867483469e | 156 | { |
worasuchad | 3:5e867483469e | 157 | directOfRobot = MeanOfYaw; |
worasuchad | 3:5e867483469e | 158 | thread2.start(servo); // Servo Thread |
worasuchad | 3:5e867483469e | 159 | pc.printf("::::::::::::::::::::::::::: Init Direction OK :::::::::::::::::::::::::::\n\r"); |
worasuchad | 3:5e867483469e | 160 | } |
worasuchad | 3:5e867483469e | 161 | |
worasuchad | 3:5e867483469e | 162 | pc.printf("directOfRobot: %.3f\n\r", directOfRobot); |
worasuchad | 3:5e867483469e | 163 | // func find diff beween directOfRobot and MeanOfYaw |
worasuchad | 3:5e867483469e | 164 | pc.printf("Diff from direction: %.3f\n\r", abs(directOfRobot - MeanOfYaw)); |
worasuchad | 3:5e867483469e | 165 | |
worasuchad | 3:5e867483469e | 166 | // reset iteration |
worasuchad | 3:5e867483469e | 167 | i = 0; |
worasuchad | 3:5e867483469e | 168 | } |
worasuchad | 3:5e867483469e | 169 | |
worasuchad | 3:5e867483469e | 170 | timer1.reset(); // reset timer |
worasuchad | 3:5e867483469e | 171 | } |
worasuchad | 2:18835f8732ad | 172 | } |
worasuchad | 3:5e867483469e | 173 | } |
worasuchad | 3:5e867483469e | 174 | |
worasuchad | 3:5e867483469e | 175 | ///////////////////////// servo /////////////////////////// |
worasuchad | 3:5e867483469e | 176 | ////////////////////////////////////////////////////////////////// |
worasuchad | 3:5e867483469e | 177 | void servo() |
worasuchad | 2:18835f8732ad | 178 | { |
worasuchad | 3:5e867483469e | 179 | Servo1.Enable(1000,20000); |
worasuchad | 3:5e867483469e | 180 | Servo2.Enable(1000,20000); |
worasuchad | 3:5e867483469e | 181 | Servo3.Enable(1000,20000); |
worasuchad | 3:5e867483469e | 182 | Servo4.Enable(1000,20000); |
worasuchad | 3:5e867483469e | 183 | |
worasuchad | 3:5e867483469e | 184 | pc.printf("Servo Start Ja!!!\n"); |
worasuchad | 3:5e867483469e | 185 | timerwalk.start(); // start timer counting |
worasuchad | 3:5e867483469e | 186 | /*pc.printf("Si\t"); |
worasuchad | 3:5e867483469e | 187 | pc.printf("%f \t",Si); |
worasuchad | 3:5e867483469e | 188 | pc.printf("down\t"); |
worasuchad | 3:5e867483469e | 189 | pc.printf("%f \t",down_degree); |
worasuchad | 3:5e867483469e | 190 | pc.printf("%f\t",Cg); |
worasuchad | 3:5e867483469e | 191 | pc.printf("up\t"); |
worasuchad | 3:5e867483469e | 192 | pc.printf("%f \t",up_degree); |
worasuchad | 3:5e867483469e | 193 | pc.printf("%f\n",Cg);*/ |
worasuchad | 3:5e867483469e | 194 | while(1) |
worasuchad | 3:5e867483469e | 195 | { |
worasuchad | 3:5e867483469e | 196 | receive_hormone(); |
worasuchad | 3:5e867483469e | 197 | cal_step_down(); // return "step_down_right" and "step_down_left" |
worasuchad | 3:5e867483469e | 198 | cal_step_up(); // return "step_up_right" and "step_up_left" |
worasuchad | 3:5e867483469e | 199 | servo_Left(); // control left lag |
worasuchad | 3:5e867483469e | 200 | servo_Right(); // control right leg |
worasuchad | 3:5e867483469e | 201 | |
worasuchad | 3:5e867483469e | 202 | // fin for walking |
worasuchad | 3:5e867483469e | 203 | if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round) |
worasuchad | 3:5e867483469e | 204 | { |
worasuchad | 3:5e867483469e | 205 | thread1.terminate(); |
worasuchad | 3:5e867483469e | 206 | pc.printf("Finish! \t"); |
worasuchad | 3:5e867483469e | 207 | walking_time = timerwalk.read_ms(); |
worasuchad | 3:5e867483469e | 208 | pc.printf("Walking time = %d ms\n\r", walking_time); |
worasuchad | 3:5e867483469e | 209 | break; |
worasuchad | 3:5e867483469e | 210 | } |
worasuchad | 3:5e867483469e | 211 | } |
worasuchad | 3:5e867483469e | 212 | } |
Khanchana | 0:43d21d5145d3 | 213 | |
worasuchad | 2:18835f8732ad | 214 | ///////////////////////// receive_hormone ///////////////// |
worasuchad | 2:18835f8732ad | 215 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 216 | void receive_hormone() |
worasuchad | 2:18835f8732ad | 217 | { |
worasuchad | 2:18835f8732ad | 218 | //down_degree = 90.00f*(1.00f-(0.06f*Cg_down)); |
worasuchad | 2:18835f8732ad | 219 | down_degree = 83.50; |
worasuchad | 2:18835f8732ad | 220 | /*pc.printf("down\t"); |
Khanchana | 1:13164a15fbf6 | 221 | pc.printf("%f \t",down_degree); |
Khanchana | 1:13164a15fbf6 | 222 | pc.printf("%f\t",Cg);*/ |
worasuchad | 2:18835f8732ad | 223 | //up_degree = 45.00f*(1.00f+(0.7f*Cg_up)); |
worasuchad | 2:18835f8732ad | 224 | up_degree = 45.00; |
Khanchana | 1:13164a15fbf6 | 225 | /*pc.printf("up\t"); |
Khanchana | 1:13164a15fbf6 | 226 | pc.printf("%f \t",up_degree); |
worasuchad | 2:18835f8732ad | 227 | pc.printf("%f\n",Cg); |
worasuchad | 2:18835f8732ad | 228 | if(down_degree < 85) |
worasuchad | 2:18835f8732ad | 229 | { |
Khanchana | 1:13164a15fbf6 | 230 | down_degree = 85; |
worasuchad | 2:18835f8732ad | 231 | if(up_degree > 75) |
worasuchad | 2:18835f8732ad | 232 | { |
worasuchad | 2:18835f8732ad | 233 | up_degree = 75; |
Khanchana | 0:43d21d5145d3 | 234 | } |
worasuchad | 2:18835f8732ad | 235 | }*/ |
Khanchana | 0:43d21d5145d3 | 236 | } |
Khanchana | 0:43d21d5145d3 | 237 | |
worasuchad | 2:18835f8732ad | 238 | ///////////////////////// cal_step_down /////////////////// |
worasuchad | 2:18835f8732ad | 239 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 240 | void cal_step_down() |
worasuchad | 2:18835f8732ad | 241 | { |
Khanchana | 0:43d21d5145d3 | 242 | //pc.printf("down"); |
Khanchana | 0:43d21d5145d3 | 243 | //pc.printf("%f \n",down_degree); |
worasuchad | 2:18835f8732ad | 244 | pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); // get degree for hormone receiver about down_degree ~ 90*, |
worasuchad | 2:18835f8732ad | 245 | 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 | 246 | if (pos_down_end_right > pos_down_end_left) |
worasuchad | 2:18835f8732ad | 247 | { |
worasuchad | 2:18835f8732ad | 248 | 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 | 249 | step_down_left = stepmin; |
worasuchad | 2:18835f8732ad | 250 | } |
worasuchad | 2:18835f8732ad | 251 | else if (pos_down_end_right < pos_down_end_left) |
worasuchad | 2:18835f8732ad | 252 | { |
Khanchana | 0:43d21d5145d3 | 253 | step_down_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 254 | step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start); |
worasuchad | 2:18835f8732ad | 255 | } |
worasuchad | 2:18835f8732ad | 256 | else // pos_down_end_right == pos_down_end_left |
worasuchad | 2:18835f8732ad | 257 | { |
Khanchana | 0:43d21d5145d3 | 258 | step_down_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 259 | step_down_left = stepmin; |
Khanchana | 0:43d21d5145d3 | 260 | } |
worasuchad | 3:5e867483469e | 261 | /*pc.printf("pos_down_right: "); |
worasuchad | 3:5e867483469e | 262 | pc.printf("%f\t\t",pos_down_end_right); |
worasuchad | 3:5e867483469e | 263 | pc.printf("pos_down_left: "); |
worasuchad | 3:5e867483469e | 264 | pc.printf("%f\t\t",pos_down_end_left); |
worasuchad | 3:5e867483469e | 265 | pc.printf("step_down_right: "); |
worasuchad | 3:5e867483469e | 266 | pc.printf("%f\t\t",step_down_right); |
worasuchad | 3:5e867483469e | 267 | pc.printf("step_down_left: "); |
worasuchad | 3:5e867483469e | 268 | pc.printf("%f\n\r",step_down_left);*/ |
Khanchana | 0:43d21d5145d3 | 269 | } |
worasuchad | 2:18835f8732ad | 270 | |
worasuchad | 2:18835f8732ad | 271 | ///////////////////////// cal_step_up ///////////////////// |
worasuchad | 2:18835f8732ad | 272 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 273 | void cal_step_up() |
worasuchad | 2:18835f8732ad | 274 | { |
Khanchana | 0:43d21d5145d3 | 275 | //pc.printf("up"); |
Khanchana | 0:43d21d5145d3 | 276 | //pc.printf("%f \n",up_degree); |
worasuchad | 2:18835f8732ad | 277 | pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); // get degree for hormone receiver about up_degree ~ 45*, |
worasuchad | 2:18835f8732ad | 278 | 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 | 279 | if (pos_up_end_right > pos_up_end_left) |
worasuchad | 2:18835f8732ad | 280 | { |
worasuchad | 2:18835f8732ad | 281 | 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 | 282 | step_up_left = stepmin; |
worasuchad | 2:18835f8732ad | 283 | } |
worasuchad | 2:18835f8732ad | 284 | else if (pos_up_end_right < pos_up_end_left) |
worasuchad | 2:18835f8732ad | 285 | { |
Khanchana | 0:43d21d5145d3 | 286 | step_up_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 287 | step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start); |
worasuchad | 2:18835f8732ad | 288 | } |
worasuchad | 2:18835f8732ad | 289 | else // step_up_right == step_up_left |
worasuchad | 2:18835f8732ad | 290 | { |
Khanchana | 0:43d21d5145d3 | 291 | step_up_right = stepmin; |
Khanchana | 0:43d21d5145d3 | 292 | step_up_left = stepmin; |
Khanchana | 0:43d21d5145d3 | 293 | } |
worasuchad | 3:5e867483469e | 294 | /*pc.printf("pos_up_right: "); |
worasuchad | 3:5e867483469e | 295 | pc.printf("%f\t\t",pos_up_end_right); |
worasuchad | 3:5e867483469e | 296 | pc.printf("pos_up_left: "); |
worasuchad | 3:5e867483469e | 297 | pc.printf("%f\t\t",pos_up_end_left); |
worasuchad | 3:5e867483469e | 298 | pc.printf("step_up_right: "); |
worasuchad | 3:5e867483469e | 299 | pc.printf("%f\t\t",step_up_right);; |
worasuchad | 3:5e867483469e | 300 | pc.printf("step_up_left: "); |
worasuchad | 3:5e867483469e | 301 | pc.printf("%f\n\r",step_up_left);*/ |
Khanchana | 0:43d21d5145d3 | 302 | } |
worasuchad | 2:18835f8732ad | 303 | |
worasuchad | 3:5e867483469e | 304 | ///////////////////////// servo_Left ////////////////////// |
worasuchad | 3:5e867483469e | 305 | ////////////////////////////////////////////////////////////////// |
worasuchad | 3:5e867483469e | 306 | void servo_Left() |
worasuchad | 2:18835f8732ad | 307 | { |
worasuchad | 3:5e867483469e | 308 | if(state_count_left == 1) |
worasuchad | 2:18835f8732ad | 309 | { |
worasuchad | 2:18835f8732ad | 310 | Servo1.SetPosition(pos_down_left); // pos_down_left = 1400.00 |
worasuchad | 2:18835f8732ad | 311 | wait(waittime); // 0.001 ms |
worasuchad | 3:5e867483469e | 312 | pos_down_left += step_down_left; |
worasuchad | 2:18835f8732ad | 313 | 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 | 314 | { |
Khanchana | 0:43d21d5145d3 | 315 | state_count_left = 2; |
Khanchana | 0:43d21d5145d3 | 316 | } |
Khanchana | 0:43d21d5145d3 | 317 | /*pc.printf("LAD"); |
Khanchana | 0:43d21d5145d3 | 318 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 319 | pc.printf("LAP"); |
Khanchana | 0:43d21d5145d3 | 320 | pc.printf("%f\n",pos_up_left);*/ |
worasuchad | 2:18835f8732ad | 321 | } |
worasuchad | 2:18835f8732ad | 322 | else if(state_count_left == 2) |
worasuchad | 2:18835f8732ad | 323 | { |
worasuchad | 2:18835f8732ad | 324 | Servo2.SetPosition(pos_up_left); // pos_up_left = 1000.00 |
Khanchana | 0:43d21d5145d3 | 325 | wait(waittime); |
worasuchad | 3:5e867483469e | 326 | pos_up_left += step_up_left; |
worasuchad | 2:18835f8732ad | 327 | 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 | 328 | { |
Khanchana | 0:43d21d5145d3 | 329 | state_count_left = 3; |
Khanchana | 0:43d21d5145d3 | 330 | } |
Khanchana | 0:43d21d5145d3 | 331 | /*pc.printf("LBD"); |
Khanchana | 0:43d21d5145d3 | 332 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 333 | pc.printf("LBP"); |
Khanchana | 0:43d21d5145d3 | 334 | pc.printf("%f\n",pos_up_left);*/ |
worasuchad | 2:18835f8732ad | 335 | } |
worasuchad | 2:18835f8732ad | 336 | else if(state_count_left == 3) |
worasuchad | 2:18835f8732ad | 337 | { |
Khanchana | 0:43d21d5145d3 | 338 | Servo1.SetPosition(pos_down_left); |
Khanchana | 0:43d21d5145d3 | 339 | wait(waittime); |
worasuchad | 3:5e867483469e | 340 | pos_down_left -= step_down_left; |
worasuchad | 2:18835f8732ad | 341 | if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) |
worasuchad | 2:18835f8732ad | 342 | { |
Khanchana | 0:43d21d5145d3 | 343 | state_count_left = 4; |
Khanchana | 0:43d21d5145d3 | 344 | } |
Khanchana | 0:43d21d5145d3 | 345 | /*pc.printf("LCD"); |
Khanchana | 0:43d21d5145d3 | 346 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 347 | pc.printf("LCP"); |
Khanchana | 0:43d21d5145d3 | 348 | pc.printf("%f\n",pos_up_left);*/ |
worasuchad | 2:18835f8732ad | 349 | } |
worasuchad | 2:18835f8732ad | 350 | else if(state_count_left == 4) |
worasuchad | 2:18835f8732ad | 351 | { |
Khanchana | 0:43d21d5145d3 | 352 | Servo2.SetPosition(pos_up_left); |
Khanchana | 0:43d21d5145d3 | 353 | wait(waittime); |
worasuchad | 3:5e867483469e | 354 | pos_up_left -= step_up_left; |
worasuchad | 2:18835f8732ad | 355 | if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) |
worasuchad | 2:18835f8732ad | 356 | { |
Khanchana | 0:43d21d5145d3 | 357 | state_count_left = 0; |
Khanchana | 0:43d21d5145d3 | 358 | } |
Khanchana | 0:43d21d5145d3 | 359 | /*pc.printf("LDD"); |
Khanchana | 0:43d21d5145d3 | 360 | pc.printf("%f\n",pos_down_left); |
Khanchana | 0:43d21d5145d3 | 361 | pc.printf("LDP"); |
Khanchana | 0:43d21d5145d3 | 362 | pc.printf("%f\n",pos_up_left);*/ |
worasuchad | 2:18835f8732ad | 363 | } |
worasuchad | 2:18835f8732ad | 364 | else if (state_count_left == 0 and round_count_left < round) |
worasuchad | 2:18835f8732ad | 365 | { |
worasuchad | 3:5e867483469e | 366 | round_count_left++; |
Khanchana | 0:43d21d5145d3 | 367 | state_count_left = 1; |
Khanchana | 0:43d21d5145d3 | 368 | pos_down_left = pos_down_start; |
Khanchana | 0:43d21d5145d3 | 369 | pos_up_left = pos_up_start; |
worasuchad | 2:18835f8732ad | 370 | } |
Khanchana | 0:43d21d5145d3 | 371 | } |
worasuchad | 2:18835f8732ad | 372 | |
worasuchad | 2:18835f8732ad | 373 | ///////////////////////// servo_Right ///////////////////// |
worasuchad | 2:18835f8732ad | 374 | ////////////////////////////////////////////////////////////////// |
Khanchana | 0:43d21d5145d3 | 375 | void servo_Right() |
Khanchana | 0:43d21d5145d3 | 376 | { |
worasuchad | 2:18835f8732ad | 377 | if(state_count_right == 1) |
worasuchad | 2:18835f8732ad | 378 | { |
Khanchana | 0:43d21d5145d3 | 379 | Servo3.SetPosition(pos_down_right); |
Khanchana | 0:43d21d5145d3 | 380 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 381 | pos_down_right = pos_down_right + step_down_right; |
worasuchad | 2:18835f8732ad | 382 | if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) |
worasuchad | 2:18835f8732ad | 383 | { |
Khanchana | 0:43d21d5145d3 | 384 | state_count_right = 2; |
Khanchana | 0:43d21d5145d3 | 385 | } |
Khanchana | 0:43d21d5145d3 | 386 | /*pc.printf("RAD"); |
Khanchana | 0:43d21d5145d3 | 387 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 388 | pc.printf("RAP"); |
Khanchana | 0:43d21d5145d3 | 389 | pc.printf("%f\n",pos_up_right);*/ |
worasuchad | 2:18835f8732ad | 390 | } |
worasuchad | 2:18835f8732ad | 391 | else if(state_count_right == 2) |
worasuchad | 2:18835f8732ad | 392 | { |
Khanchana | 0:43d21d5145d3 | 393 | Servo4.SetPosition(pos_up_right); |
Khanchana | 0:43d21d5145d3 | 394 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 395 | pos_up_right = pos_up_right + step_up_right; |
worasuchad | 2:18835f8732ad | 396 | 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 | 397 | { |
Khanchana | 0:43d21d5145d3 | 398 | state_count_right = 3; |
Khanchana | 0:43d21d5145d3 | 399 | } |
Khanchana | 0:43d21d5145d3 | 400 | /*pc.printf("RBD"); |
Khanchana | 0:43d21d5145d3 | 401 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 402 | pc.printf("RBP"); |
Khanchana | 0:43d21d5145d3 | 403 | pc.printf("%f\n",pos_up_right);*/ |
worasuchad | 2:18835f8732ad | 404 | } |
worasuchad | 2:18835f8732ad | 405 | else if(state_count_right == 3) |
worasuchad | 2:18835f8732ad | 406 | { |
Khanchana | 0:43d21d5145d3 | 407 | Servo3.SetPosition(pos_down_right); |
Khanchana | 0:43d21d5145d3 | 408 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 409 | pos_down_right = pos_down_right - step_down_right; |
worasuchad | 2:18835f8732ad | 410 | if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) |
worasuchad | 2:18835f8732ad | 411 | { |
Khanchana | 0:43d21d5145d3 | 412 | state_count_right = 4; |
Khanchana | 0:43d21d5145d3 | 413 | } |
Khanchana | 0:43d21d5145d3 | 414 | /*pc.printf("RCD"); |
Khanchana | 0:43d21d5145d3 | 415 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 416 | pc.printf("RCP"); |
Khanchana | 0:43d21d5145d3 | 417 | pc.printf("%f\n",pos_up_right);*/ |
worasuchad | 2:18835f8732ad | 418 | } |
worasuchad | 2:18835f8732ad | 419 | else if(state_count_right == 4) |
worasuchad | 2:18835f8732ad | 420 | { |
Khanchana | 0:43d21d5145d3 | 421 | Servo4.SetPosition(pos_up_right); |
worasuchad | 2:18835f8732ad | 422 | wait(waittime); |
Khanchana | 0:43d21d5145d3 | 423 | pos_up_right = pos_up_right - step_up_right; |
worasuchad | 2:18835f8732ad | 424 | if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) |
worasuchad | 2:18835f8732ad | 425 | { |
Khanchana | 0:43d21d5145d3 | 426 | state_count_right = 0; |
Khanchana | 0:43d21d5145d3 | 427 | } |
Khanchana | 0:43d21d5145d3 | 428 | /*pc.printf("RDD"); |
Khanchana | 0:43d21d5145d3 | 429 | pc.printf("%f\n",pos_down_right); |
Khanchana | 0:43d21d5145d3 | 430 | pc.printf("RDP"); |
Khanchana | 0:43d21d5145d3 | 431 | pc.printf("%f\n",pos_up_right);*/ |
worasuchad | 2:18835f8732ad | 432 | } |
worasuchad | 2:18835f8732ad | 433 | else if (state_count_right == 0 and round_count_right < round) |
worasuchad | 2:18835f8732ad | 434 | { |
Khanchana | 0:43d21d5145d3 | 435 | round_count_right = round_count_right+1; |
Khanchana | 0:43d21d5145d3 | 436 | state_count_right = 1; |
Khanchana | 0:43d21d5145d3 | 437 | pos_down_right = pos_down_start; |
Khanchana | 0:43d21d5145d3 | 438 | pos_up_right = pos_up_start; |
Khanchana | 0:43d21d5145d3 | 439 | } |
Khanchana | 0:43d21d5145d3 | 440 | } |
Khanchana | 0:43d21d5145d3 | 441 | |
worasuchad | 2:18835f8732ad | 442 | ///////////////// Hormone Concentration ///////////////////// |
worasuchad | 2:18835f8732ad | 443 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 444 | float HormoneCon(float SiPreProcess) |
worasuchad | 2:18835f8732ad | 445 | { |
worasuchad | 2:18835f8732ad | 446 | float CgTemp; |
worasuchad | 2:18835f8732ad | 447 | |
worasuchad | 2:18835f8732ad | 448 | //pc.printf("SiPreProcess: %.3f\t", SiPreProcess); |
worasuchad | 2:18835f8732ad | 449 | //pc.printf("CgPrevious: %.3f\t", CgPrevious); |
worasuchad | 2:18835f8732ad | 450 | |
worasuchad | 2:18835f8732ad | 451 | ////// hormone gland ////// |
worasuchad | 2:18835f8732ad | 452 | CgTemp = (0.8f*SiPreProcess) + (0.5f*CgPrevious); |
worasuchad | 2:18835f8732ad | 453 | //pc.printf("CgTemp: %.3f\t", CgTemp); |
worasuchad | 2:18835f8732ad | 454 | Cg = 1/( 1+exp(-CgTemp) ); // used sigmoid func for calculating Cg which much have value between 0 - 1 |
worasuchad | 2:18835f8732ad | 455 | /////////////////////////// |
worasuchad | 2:18835f8732ad | 456 | |
worasuchad | 2:18835f8732ad | 457 | //pc.printf("First Term: %.3f\t", (float)(0.8f*SiPreProcess)); |
worasuchad | 2:18835f8732ad | 458 | //pc.printf("Secound Term: %.3f\t", (float)(0.5f*CgPrevious)); |
worasuchad | 2:18835f8732ad | 459 | CgPrevious = Cg; |
worasuchad | 2:18835f8732ad | 460 | //*********// |
worasuchad | 2:18835f8732ad | 461 | Cg_down = Cg; |
worasuchad | 2:18835f8732ad | 462 | Cg_up = Cg; |
worasuchad | 2:18835f8732ad | 463 | |
worasuchad | 2:18835f8732ad | 464 | return Cg; |
Khanchana | 0:43d21d5145d3 | 465 | } |
Khanchana | 0:43d21d5145d3 | 466 | |
worasuchad | 2:18835f8732ad | 467 | ///////////////////////// calculateSD ///////////////////// |
worasuchad | 2:18835f8732ad | 468 | ////////////////////////////////////////////////////////////////// |
worasuchad | 2:18835f8732ad | 469 | float calculateSD(float sdData[]) |
worasuchad | 2:18835f8732ad | 470 | { |
worasuchad | 2:18835f8732ad | 471 | float sum = 0.0, mean, standardDeviation = 0.0; |
worasuchad | 2:18835f8732ad | 472 | int i; |
worasuchad | 2:18835f8732ad | 473 | |
worasuchad | 3:5e867483469e | 474 | for(i = 0; i < 10 ; ++i) |
worasuchad | 2:18835f8732ad | 475 | { |
worasuchad | 2:18835f8732ad | 476 | sum += sdData[i]; |
worasuchad | 2:18835f8732ad | 477 | //pc.printf("sum = %.2f \n\r",sum); |
worasuchad | 2:18835f8732ad | 478 | } |
worasuchad | 2:18835f8732ad | 479 | mean = sum/10; |
worasuchad | 2:18835f8732ad | 480 | //pc.printf("mean = %.2f \n\r",mean); |
worasuchad | 2:18835f8732ad | 481 | |
worasuchad | 3:5e867483469e | 482 | for(i = 0; i < 10; ++i) |
worasuchad | 2:18835f8732ad | 483 | { |
worasuchad | 2:18835f8732ad | 484 | standardDeviation += pow(sdData[i] - mean, 2); |
worasuchad | 2:18835f8732ad | 485 | //pc.printf("standardDeviation = %.2f \n\r",standardDeviation); |
Khanchana | 0:43d21d5145d3 | 486 | } |
worasuchad | 2:18835f8732ad | 487 | return sqrt(standardDeviation / 10); |
worasuchad | 3:5e867483469e | 488 | } |
worasuchad | 3:5e867483469e | 489 | |
worasuchad | 3:5e867483469e | 490 | ///////////////////////// calculateMean /////////////////// |
worasuchad | 3:5e867483469e | 491 | ////////////////////////////////////////////////////////////////// |
worasuchad | 3:5e867483469e | 492 | float calculateMean(float meanData[]) |
worasuchad | 3:5e867483469e | 493 | { |
worasuchad | 3:5e867483469e | 494 | float sum = 0.0, mean; |
worasuchad | 3:5e867483469e | 495 | int i; |
worasuchad | 3:5e867483469e | 496 | |
worasuchad | 3:5e867483469e | 497 | for(i = 0; i < 10 ; ++i) |
worasuchad | 3:5e867483469e | 498 | { |
worasuchad | 3:5e867483469e | 499 | sum += meanData[i]; |
worasuchad | 3:5e867483469e | 500 | //pc.printf("sum = %.2f \n\r",sum); |
worasuchad | 3:5e867483469e | 501 | } |
worasuchad | 3:5e867483469e | 502 | mean = sum/10; |
worasuchad | 3:5e867483469e | 503 | //pc.printf("mean = %.2f \n\r",mean); |
worasuchad | 3:5e867483469e | 504 | return mean; |
worasuchad | 3:5e867483469e | 505 | } |
worasuchad | 3:5e867483469e | 506 | |
worasuchad | 3:5e867483469e | 507 | ///////////////////////// directionRobot ////////////////// |
worasuchad | 3:5e867483469e | 508 | ////////////////////////////////////////////////////////////////// |
worasuchad | 3:5e867483469e | 509 | bool directionRobot(float directionMean, float directionSD) |
worasuchad | 3:5e867483469e | 510 | { |
worasuchad | 3:5e867483469e | 511 | if( directionSD < 0.06f and initDirection == 0 ) |
worasuchad | 3:5e867483469e | 512 | { |
worasuchad | 3:5e867483469e | 513 | initDirection++; |
worasuchad | 3:5e867483469e | 514 | return 1; |
worasuchad | 3:5e867483469e | 515 | } |
worasuchad | 3:5e867483469e | 516 | return 0; |
Khanchana | 0:43d21d5145d3 | 517 | } |