TurtleBot / Mbed 2 deprecated TurtleBot_v01

turtlebot v 01

Dependencies:   Servo mbed-rtos mbed PM hormone

Fork of TurtleBot_v01 by worasuchad haomachai

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?

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 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 }