worasuchad haomachai / Mbed 2 deprecated TurtleBot_v01

20/6/18 experiment wt hormone

Dependencies:   Servo TurtleBot_V6 mbed-rtos mbed

Fork of TurtleBot_v00 by worasuchad haomachai

main.cpp@4:ec7e68b84f2b, 2018-06-21 (annotated)

Committer:
worasuchad
Date:
Thu Jun 21 08:12:44 2018 +0000
Revision:
4:ec7e68b84f2b
Parent:
3:5e867483469e 
20/6/18

Who changed what in which revision?

UserRevisionLine numberNew 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 4:ec7e68b84f2b 157 directOfRobot = MeanOfYaw; // direction of robot
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 4:ec7e68b84f2b 161 pc.printf("directOfRobot: %.3f\n\r", directOfRobot);
worasuchad 3:5e867483469e 162
worasuchad 4:ec7e68b84f2b 163 if(directOfRobot > SDOfYaw)
worasuchad 4:ec7e68b84f2b 164 {
worasuchad 4:ec7e68b84f2b 165 pc.printf("Turn Left: %.3f\n\r", (directOfRobot - MeanOfYaw));
worasuchad 4:ec7e68b84f2b 166 }
worasuchad 4:ec7e68b84f2b 167 else
worasuchad 4:ec7e68b84f2b 168 {
worasuchad 4:ec7e68b84f2b 169 pc.printf("Turn Right: %.3f\n\r", (MeanOfYaw - directOfRobot));
worasuchad 4:ec7e68b84f2b 170 }
worasuchad 4:ec7e68b84f2b 171
worasuchad 3:5e867483469e 172 // func find diff beween directOfRobot and MeanOfYaw
worasuchad 4:ec7e68b84f2b 173 //pc.printf("Diff from direction: %.3f\n\r", abs(directOfRobot - MeanOfYaw));
worasuchad 3:5e867483469e 174
worasuchad 3:5e867483469e 175 // reset iteration
worasuchad 3:5e867483469e 176 i = 0;
worasuchad 3:5e867483469e 177 }
worasuchad 3:5e867483469e 178
worasuchad 3:5e867483469e 179 timer1.reset(); // reset timer
worasuchad 3:5e867483469e 180 }
worasuchad 2:18835f8732ad 181 }
worasuchad 3:5e867483469e 182 }
worasuchad 3:5e867483469e 183
worasuchad 3:5e867483469e 184 ///////////////////////// servo ///////////////////////////
worasuchad 3:5e867483469e 185 //////////////////////////////////////////////////////////////////
worasuchad 3:5e867483469e 186 void servo()
worasuchad 2:18835f8732ad 187 {
worasuchad 3:5e867483469e 188 Servo1.Enable(1000,20000);
worasuchad 3:5e867483469e 189 Servo2.Enable(1000,20000);
worasuchad 3:5e867483469e 190 Servo3.Enable(1000,20000);
worasuchad 3:5e867483469e 191 Servo4.Enable(1000,20000);
worasuchad 3:5e867483469e 192
worasuchad 3:5e867483469e 193 pc.printf("Servo Start Ja!!!\n");
worasuchad 3:5e867483469e 194 timerwalk.start(); // start timer counting
worasuchad 3:5e867483469e 195 /*pc.printf("Si\t");
worasuchad 3:5e867483469e 196 pc.printf("%f \t",Si);
worasuchad 3:5e867483469e 197 pc.printf("down\t");
worasuchad 3:5e867483469e 198 pc.printf("%f \t",down_degree);
worasuchad 3:5e867483469e 199 pc.printf("%f\t",Cg);
worasuchad 3:5e867483469e 200 pc.printf("up\t");
worasuchad 3:5e867483469e 201 pc.printf("%f \t",up_degree);
worasuchad 3:5e867483469e 202 pc.printf("%f\n",Cg);*/
worasuchad 3:5e867483469e 203 while(1)
worasuchad 3:5e867483469e 204 {
worasuchad 3:5e867483469e 205 receive_hormone();
worasuchad 3:5e867483469e 206 cal_step_down(); // return "step_down_right" and "step_down_left"
worasuchad 3:5e867483469e 207 cal_step_up(); // return "step_up_right" and "step_up_left"
worasuchad 3:5e867483469e 208 servo_Left(); // control left lag
worasuchad 3:5e867483469e 209 servo_Right(); // control right leg
worasuchad 3:5e867483469e 210
worasuchad 3:5e867483469e 211 // fin for walking
worasuchad 3:5e867483469e 212 if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round)
worasuchad 3:5e867483469e 213 {
worasuchad 3:5e867483469e 214 thread1.terminate();
worasuchad 3:5e867483469e 215 pc.printf("Finish! \t");
worasuchad 3:5e867483469e 216 walking_time = timerwalk.read_ms();
worasuchad 3:5e867483469e 217 pc.printf("Walking time = %d ms\n\r", walking_time);
worasuchad 3:5e867483469e 218 break;
worasuchad 3:5e867483469e 219 }
worasuchad 3:5e867483469e 220 }
worasuchad 3:5e867483469e 221 }
Khanchana 0:43d21d5145d3 222
worasuchad 2:18835f8732ad 223 ///////////////////////// receive_hormone /////////////////
worasuchad 2:18835f8732ad 224 //////////////////////////////////////////////////////////////////
worasuchad 2:18835f8732ad 225 void receive_hormone()
worasuchad 2:18835f8732ad 226 {
worasuchad 2:18835f8732ad 227 //down_degree = 90.00f*(1.00f-(0.06f*Cg_down));
worasuchad 2:18835f8732ad 228 down_degree = 83.50;
worasuchad 2:18835f8732ad 229 /*pc.printf("down\t");
Khanchana 1:13164a15fbf6 230 pc.printf("%f \t",down_degree);
Khanchana 1:13164a15fbf6 231 pc.printf("%f\t",Cg);*/
worasuchad 2:18835f8732ad 232 //up_degree = 45.00f*(1.00f+(0.7f*Cg_up));
worasuchad 2:18835f8732ad 233 up_degree = 45.00;
Khanchana 1:13164a15fbf6 234 /*pc.printf("up\t");
Khanchana 1:13164a15fbf6 235 pc.printf("%f \t",up_degree);
worasuchad 2:18835f8732ad 236 pc.printf("%f\n",Cg);
worasuchad 2:18835f8732ad 237 if(down_degree < 85)
worasuchad 2:18835f8732ad 238 {
Khanchana 1:13164a15fbf6 239 down_degree = 85;
worasuchad 2:18835f8732ad 240 if(up_degree > 75)
worasuchad 2:18835f8732ad 241 {
worasuchad 2:18835f8732ad 242 up_degree = 75;
Khanchana 0:43d21d5145d3 243 }
worasuchad 2:18835f8732ad 244 }*/
Khanchana 0:43d21d5145d3 245 }
Khanchana 0:43d21d5145d3 246
worasuchad 2:18835f8732ad 247 ///////////////////////// cal_step_down ///////////////////
worasuchad 2:18835f8732ad 248 //////////////////////////////////////////////////////////////////
worasuchad 2:18835f8732ad 249 void cal_step_down()
worasuchad 2:18835f8732ad 250 {
Khanchana 0:43d21d5145d3 251 //pc.printf("down");
Khanchana 0:43d21d5145d3 252 //pc.printf("%f \n",down_degree);
worasuchad 2:18835f8732ad 253 pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); // get degree for hormone receiver about down_degree ~ 90*,
worasuchad 2:18835f8732ad 254 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 255 if (pos_down_end_right > pos_down_end_left)
worasuchad 2:18835f8732ad 256 {
worasuchad 2:18835f8732ad 257 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 258 step_down_left = stepmin;
worasuchad 2:18835f8732ad 259 }
worasuchad 2:18835f8732ad 260 else if (pos_down_end_right < pos_down_end_left)
worasuchad 2:18835f8732ad 261 {
Khanchana 0:43d21d5145d3 262 step_down_right = stepmin;
Khanchana 0:43d21d5145d3 263 step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
worasuchad 2:18835f8732ad 264 }
worasuchad 2:18835f8732ad 265 else // pos_down_end_right == pos_down_end_left
worasuchad 2:18835f8732ad 266 {
Khanchana 0:43d21d5145d3 267 step_down_right = stepmin;
Khanchana 0:43d21d5145d3 268 step_down_left = stepmin;
Khanchana 0:43d21d5145d3 269 }
worasuchad 3:5e867483469e 270 /*pc.printf("pos_down_right: ");
worasuchad 3:5e867483469e 271 pc.printf("%f\t\t",pos_down_end_right);
worasuchad 3:5e867483469e 272 pc.printf("pos_down_left: ");
worasuchad 3:5e867483469e 273 pc.printf("%f\t\t",pos_down_end_left);
worasuchad 3:5e867483469e 274 pc.printf("step_down_right: ");
worasuchad 3:5e867483469e 275 pc.printf("%f\t\t",step_down_right);
worasuchad 3:5e867483469e 276 pc.printf("step_down_left: ");
worasuchad 3:5e867483469e 277 pc.printf("%f\n\r",step_down_left);*/
Khanchana 0:43d21d5145d3 278 }
worasuchad 2:18835f8732ad 279
worasuchad 2:18835f8732ad 280 ///////////////////////// cal_step_up /////////////////////
worasuchad 2:18835f8732ad 281 //////////////////////////////////////////////////////////////////
worasuchad 2:18835f8732ad 282 void cal_step_up()
worasuchad 2:18835f8732ad 283 {
Khanchana 0:43d21d5145d3 284 //pc.printf("up");
Khanchana 0:43d21d5145d3 285 //pc.printf("%f \n",up_degree);
worasuchad 2:18835f8732ad 286 pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); // get degree for hormone receiver about up_degree ~ 45*,
worasuchad 2:18835f8732ad 287 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 288 if (pos_up_end_right > pos_up_end_left)
worasuchad 2:18835f8732ad 289 {
worasuchad 2:18835f8732ad 290 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 291 step_up_left = stepmin;
worasuchad 2:18835f8732ad 292 }
worasuchad 2:18835f8732ad 293 else if (pos_up_end_right < pos_up_end_left)
worasuchad 2:18835f8732ad 294 {
Khanchana 0:43d21d5145d3 295 step_up_right = stepmin;
Khanchana 0:43d21d5145d3 296 step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
worasuchad 2:18835f8732ad 297 }
worasuchad 2:18835f8732ad 298 else // step_up_right == step_up_left
worasuchad 2:18835f8732ad 299 {
Khanchana 0:43d21d5145d3 300 step_up_right = stepmin;
Khanchana 0:43d21d5145d3 301 step_up_left = stepmin;
Khanchana 0:43d21d5145d3 302 }
worasuchad 3:5e867483469e 303 /*pc.printf("pos_up_right: ");
worasuchad 3:5e867483469e 304 pc.printf("%f\t\t",pos_up_end_right);
worasuchad 3:5e867483469e 305 pc.printf("pos_up_left: ");
worasuchad 3:5e867483469e 306 pc.printf("%f\t\t",pos_up_end_left);
worasuchad 3:5e867483469e 307 pc.printf("step_up_right: ");
worasuchad 3:5e867483469e 308 pc.printf("%f\t\t",step_up_right);;
worasuchad 3:5e867483469e 309 pc.printf("step_up_left: ");
worasuchad 3:5e867483469e 310 pc.printf("%f\n\r",step_up_left);*/
Khanchana 0:43d21d5145d3 311 }
worasuchad 2:18835f8732ad 312
worasuchad 3:5e867483469e 313 ///////////////////////// servo_Left //////////////////////
worasuchad 3:5e867483469e 314 //////////////////////////////////////////////////////////////////
worasuchad 3:5e867483469e 315 void servo_Left()
worasuchad 2:18835f8732ad 316 {
worasuchad 3:5e867483469e 317 if(state_count_left == 1)
worasuchad 2:18835f8732ad 318 {
worasuchad 2:18835f8732ad 319 Servo1.SetPosition(pos_down_left); // pos_down_left = 1400.00
worasuchad 2:18835f8732ad 320 wait(waittime); // 0.001 ms
worasuchad 3:5e867483469e 321 pos_down_left += step_down_left;
worasuchad 2:18835f8732ad 322 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 323 {
Khanchana 0:43d21d5145d3 324 state_count_left = 2;
Khanchana 0:43d21d5145d3 325 }
Khanchana 0:43d21d5145d3 326 /*pc.printf("LAD");
Khanchana 0:43d21d5145d3 327 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 328 pc.printf("LAP");
Khanchana 0:43d21d5145d3 329 pc.printf("%f\n",pos_up_left);*/
worasuchad 2:18835f8732ad 330 }
worasuchad 2:18835f8732ad 331 else if(state_count_left == 2)
worasuchad 2:18835f8732ad 332 {
worasuchad 2:18835f8732ad 333 Servo2.SetPosition(pos_up_left); // pos_up_left = 1000.00
Khanchana 0:43d21d5145d3 334 wait(waittime);
worasuchad 3:5e867483469e 335 pos_up_left += step_up_left;
worasuchad 2:18835f8732ad 336 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 337 {
Khanchana 0:43d21d5145d3 338 state_count_left = 3;
Khanchana 0:43d21d5145d3 339 }
Khanchana 0:43d21d5145d3 340 /*pc.printf("LBD");
Khanchana 0:43d21d5145d3 341 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 342 pc.printf("LBP");
Khanchana 0:43d21d5145d3 343 pc.printf("%f\n",pos_up_left);*/
worasuchad 2:18835f8732ad 344 }
worasuchad 2:18835f8732ad 345 else if(state_count_left == 3)
worasuchad 2:18835f8732ad 346 {
Khanchana 0:43d21d5145d3 347 Servo1.SetPosition(pos_down_left);
Khanchana 0:43d21d5145d3 348 wait(waittime);
worasuchad 3:5e867483469e 349 pos_down_left -= step_down_left;
worasuchad 2:18835f8732ad 350 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left)
worasuchad 2:18835f8732ad 351 {
Khanchana 0:43d21d5145d3 352 state_count_left = 4;
Khanchana 0:43d21d5145d3 353 }
Khanchana 0:43d21d5145d3 354 /*pc.printf("LCD");
Khanchana 0:43d21d5145d3 355 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 356 pc.printf("LCP");
Khanchana 0:43d21d5145d3 357 pc.printf("%f\n",pos_up_left);*/
worasuchad 2:18835f8732ad 358 }
worasuchad 2:18835f8732ad 359 else if(state_count_left == 4)
worasuchad 2:18835f8732ad 360 {
Khanchana 0:43d21d5145d3 361 Servo2.SetPosition(pos_up_left);
Khanchana 0:43d21d5145d3 362 wait(waittime);
worasuchad 3:5e867483469e 363 pos_up_left -= step_up_left;
worasuchad 2:18835f8732ad 364 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left)
worasuchad 2:18835f8732ad 365 {
Khanchana 0:43d21d5145d3 366 state_count_left = 0;
Khanchana 0:43d21d5145d3 367 }
Khanchana 0:43d21d5145d3 368 /*pc.printf("LDD");
Khanchana 0:43d21d5145d3 369 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 370 pc.printf("LDP");
Khanchana 0:43d21d5145d3 371 pc.printf("%f\n",pos_up_left);*/
worasuchad 2:18835f8732ad 372 }
worasuchad 2:18835f8732ad 373 else if (state_count_left == 0 and round_count_left < round)
worasuchad 2:18835f8732ad 374 {
worasuchad 3:5e867483469e 375 round_count_left++;
Khanchana 0:43d21d5145d3 376 state_count_left = 1;
Khanchana 0:43d21d5145d3 377 pos_down_left = pos_down_start;
Khanchana 0:43d21d5145d3 378 pos_up_left = pos_up_start;
worasuchad 2:18835f8732ad 379 }
Khanchana 0:43d21d5145d3 380 }
worasuchad 2:18835f8732ad 381
worasuchad 2:18835f8732ad 382 ///////////////////////// servo_Right /////////////////////
worasuchad 2:18835f8732ad 383 //////////////////////////////////////////////////////////////////
Khanchana 0:43d21d5145d3 384 void servo_Right()
Khanchana 0:43d21d5145d3 385 {
worasuchad 2:18835f8732ad 386 if(state_count_right == 1)
worasuchad 2:18835f8732ad 387 {
Khanchana 0:43d21d5145d3 388 Servo3.SetPosition(pos_down_right);
Khanchana 0:43d21d5145d3 389 wait(waittime);
Khanchana 0:43d21d5145d3 390 pos_down_right = pos_down_right + step_down_right;
worasuchad 2:18835f8732ad 391 if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start)
worasuchad 2:18835f8732ad 392 {
Khanchana 0:43d21d5145d3 393 state_count_right = 2;
Khanchana 0:43d21d5145d3 394 }
Khanchana 0:43d21d5145d3 395 /*pc.printf("RAD");
Khanchana 0:43d21d5145d3 396 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 397 pc.printf("RAP");
Khanchana 0:43d21d5145d3 398 pc.printf("%f\n",pos_up_right);*/
worasuchad 2:18835f8732ad 399 }
worasuchad 2:18835f8732ad 400 else if(state_count_right == 2)
worasuchad 2:18835f8732ad 401 {
Khanchana 0:43d21d5145d3 402 Servo4.SetPosition(pos_up_right);
Khanchana 0:43d21d5145d3 403 wait(waittime);
Khanchana 0:43d21d5145d3 404 pos_up_right = pos_up_right + step_up_right;
worasuchad 2:18835f8732ad 405 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 406 {
Khanchana 0:43d21d5145d3 407 state_count_right = 3;
Khanchana 0:43d21d5145d3 408 }
Khanchana 0:43d21d5145d3 409 /*pc.printf("RBD");
Khanchana 0:43d21d5145d3 410 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 411 pc.printf("RBP");
Khanchana 0:43d21d5145d3 412 pc.printf("%f\n",pos_up_right);*/
worasuchad 2:18835f8732ad 413 }
worasuchad 2:18835f8732ad 414 else if(state_count_right == 3)
worasuchad 2:18835f8732ad 415 {
Khanchana 0:43d21d5145d3 416 Servo3.SetPosition(pos_down_right);
Khanchana 0:43d21d5145d3 417 wait(waittime);
Khanchana 0:43d21d5145d3 418 pos_down_right = pos_down_right - step_down_right;
worasuchad 2:18835f8732ad 419 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right)
worasuchad 2:18835f8732ad 420 {
Khanchana 0:43d21d5145d3 421 state_count_right = 4;
Khanchana 0:43d21d5145d3 422 }
Khanchana 0:43d21d5145d3 423 /*pc.printf("RCD");
Khanchana 0:43d21d5145d3 424 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 425 pc.printf("RCP");
Khanchana 0:43d21d5145d3 426 pc.printf("%f\n",pos_up_right);*/
worasuchad 2:18835f8732ad 427 }
worasuchad 2:18835f8732ad 428 else if(state_count_right == 4)
worasuchad 2:18835f8732ad 429 {
Khanchana 0:43d21d5145d3 430 Servo4.SetPosition(pos_up_right);
worasuchad 2:18835f8732ad 431 wait(waittime);
Khanchana 0:43d21d5145d3 432 pos_up_right = pos_up_right - step_up_right;
worasuchad 2:18835f8732ad 433 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right)
worasuchad 2:18835f8732ad 434 {
Khanchana 0:43d21d5145d3 435 state_count_right = 0;
Khanchana 0:43d21d5145d3 436 }
Khanchana 0:43d21d5145d3 437 /*pc.printf("RDD");
Khanchana 0:43d21d5145d3 438 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 439 pc.printf("RDP");
Khanchana 0:43d21d5145d3 440 pc.printf("%f\n",pos_up_right);*/
worasuchad 2:18835f8732ad 441 }
worasuchad 2:18835f8732ad 442 else if (state_count_right == 0 and round_count_right < round)
worasuchad 2:18835f8732ad 443 {
Khanchana 0:43d21d5145d3 444 round_count_right = round_count_right+1;
Khanchana 0:43d21d5145d3 445 state_count_right = 1;
Khanchana 0:43d21d5145d3 446 pos_down_right = pos_down_start;
Khanchana 0:43d21d5145d3 447 pos_up_right = pos_up_start;
Khanchana 0:43d21d5145d3 448 }
Khanchana 0:43d21d5145d3 449 }
Khanchana 0:43d21d5145d3 450
worasuchad 2:18835f8732ad 451 ///////////////// Hormone Concentration /////////////////////
worasuchad 2:18835f8732ad 452 //////////////////////////////////////////////////////////////////
worasuchad 2:18835f8732ad 453 float HormoneCon(float SiPreProcess)
worasuchad 2:18835f8732ad 454 {
worasuchad 2:18835f8732ad 455 float CgTemp;
worasuchad 2:18835f8732ad 456
worasuchad 2:18835f8732ad 457 //pc.printf("SiPreProcess: %.3f\t", SiPreProcess);
worasuchad 2:18835f8732ad 458 //pc.printf("CgPrevious: %.3f\t", CgPrevious);
worasuchad 2:18835f8732ad 459
worasuchad 2:18835f8732ad 460 ////// hormone gland //////
worasuchad 2:18835f8732ad 461 CgTemp = (0.8f*SiPreProcess) + (0.5f*CgPrevious);
worasuchad 2:18835f8732ad 462 //pc.printf("CgTemp: %.3f\t", CgTemp);
worasuchad 2:18835f8732ad 463 Cg = 1/( 1+exp(-CgTemp) ); // used sigmoid func for calculating Cg which much have value between 0 - 1
worasuchad 2:18835f8732ad 464 ///////////////////////////
worasuchad 2:18835f8732ad 465
worasuchad 2:18835f8732ad 466 //pc.printf("First Term: %.3f\t", (float)(0.8f*SiPreProcess));
worasuchad 2:18835f8732ad 467 //pc.printf("Secound Term: %.3f\t", (float)(0.5f*CgPrevious));
worasuchad 2:18835f8732ad 468 CgPrevious = Cg;
worasuchad 2:18835f8732ad 469 //*********//
worasuchad 2:18835f8732ad 470 Cg_down = Cg;
worasuchad 2:18835f8732ad 471 Cg_up = Cg;
worasuchad 2:18835f8732ad 472
worasuchad 2:18835f8732ad 473 return Cg;
Khanchana 0:43d21d5145d3 474 }
Khanchana 0:43d21d5145d3 475
worasuchad 2:18835f8732ad 476 ///////////////////////// calculateSD /////////////////////
worasuchad 2:18835f8732ad 477 //////////////////////////////////////////////////////////////////
worasuchad 2:18835f8732ad 478 float calculateSD(float sdData[])
worasuchad 2:18835f8732ad 479 {
worasuchad 2:18835f8732ad 480 float sum = 0.0, mean, standardDeviation = 0.0;
worasuchad 2:18835f8732ad 481 int i;
worasuchad 2:18835f8732ad 482
worasuchad 3:5e867483469e 483 for(i = 0; i < 10 ; ++i)
worasuchad 2:18835f8732ad 484 {
worasuchad 2:18835f8732ad 485 sum += sdData[i];
worasuchad 2:18835f8732ad 486 //pc.printf("sum = %.2f \n\r",sum);
worasuchad 2:18835f8732ad 487 }
worasuchad 2:18835f8732ad 488 mean = sum/10;
worasuchad 2:18835f8732ad 489 //pc.printf("mean = %.2f \n\r",mean);
worasuchad 2:18835f8732ad 490
worasuchad 3:5e867483469e 491 for(i = 0; i < 10; ++i)
worasuchad 2:18835f8732ad 492 {
worasuchad 2:18835f8732ad 493 standardDeviation += pow(sdData[i] - mean, 2);
worasuchad 2:18835f8732ad 494 //pc.printf("standardDeviation = %.2f \n\r",standardDeviation);
Khanchana 0:43d21d5145d3 495 }
worasuchad 2:18835f8732ad 496 return sqrt(standardDeviation / 10);
worasuchad 3:5e867483469e 497 }
worasuchad 3:5e867483469e 498
worasuchad 3:5e867483469e 499 ///////////////////////// calculateMean ///////////////////
worasuchad 3:5e867483469e 500 //////////////////////////////////////////////////////////////////
worasuchad 3:5e867483469e 501 float calculateMean(float meanData[])
worasuchad 3:5e867483469e 502 {
worasuchad 3:5e867483469e 503 float sum = 0.0, mean;
worasuchad 3:5e867483469e 504 int i;
worasuchad 3:5e867483469e 505
worasuchad 3:5e867483469e 506 for(i = 0; i < 10 ; ++i)
worasuchad 3:5e867483469e 507 {
worasuchad 3:5e867483469e 508 sum += meanData[i];
worasuchad 3:5e867483469e 509 //pc.printf("sum = %.2f \n\r",sum);
worasuchad 3:5e867483469e 510 }
worasuchad 3:5e867483469e 511 mean = sum/10;
worasuchad 3:5e867483469e 512 //pc.printf("mean = %.2f \n\r",mean);
worasuchad 3:5e867483469e 513 return mean;
worasuchad 3:5e867483469e 514 }
worasuchad 3:5e867483469e 515
worasuchad 3:5e867483469e 516 ///////////////////////// directionRobot //////////////////
worasuchad 3:5e867483469e 517 //////////////////////////////////////////////////////////////////
worasuchad 3:5e867483469e 518 bool directionRobot(float directionMean, float directionSD)
worasuchad 3:5e867483469e 519 {
worasuchad 3:5e867483469e 520 if( directionSD < 0.06f and initDirection == 0 )
worasuchad 3:5e867483469e 521 {
worasuchad 3:5e867483469e 522 initDirection++;
worasuchad 3:5e867483469e 523 return 1;
worasuchad 3:5e867483469e 524 }
worasuchad 3:5e867483469e 525 return 0;
Khanchana 0:43d21d5145d3 526 }