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@5:08334c6a42ca, 2018-07-11 (annotated)

Committer:
worasuchad
Date:
Wed Jul 11 05:38:01 2018 +0000
Revision:
5:08334c6a42ca
Parent:
4:ec7e68b84f2b
Child:
6:8ae55e1f7e76
turtlebot_v01

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 5:08334c6a42ca 7 // detail : //
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 5:08334c6a42ca 34 void servoFirstState();
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 5:08334c6a42ca 39
Khanchana 0:43d21d5145d3 40 void receive_hormone();
worasuchad 2:18835f8732ad 41 float calculateSD(float data[]);
worasuchad 3:5e867483469e 42 float calculateMean(float meanData[]);
worasuchad 5:08334c6a42ca 43 float HormoneCon(float );
worasuchad 5:08334c6a42ca 44 bool checkIMUFirst(float , float , float );
worasuchad 5:08334c6a42ca 45
worasuchad 5:08334c6a42ca 46 ////// debug func //////
worasuchad 5:08334c6a42ca 47 void printStateGait();
worasuchad 2:18835f8732ad 48
worasuchad 2:18835f8732ad 49 ///////////////////////// variable ///////////////////////////////
worasuchad 2:18835f8732ad 50 //////////////////////////////////////////////////////////////////
worasuchad 3:5e867483469e 51 // home param
Khanchana 0:43d21d5145d3 52 int walking_time;
worasuchad 5:08334c6a42ca 53 int initCheck = 0;
worasuchad 5:08334c6a42ca 54 float waittime = 0.001 ;
worasuchad 5:08334c6a42ca 55 float round = 6;
Khanchana 0:43d21d5145d3 56
worasuchad 3:5e867483469e 57 // hormone param
Khanchana 1:13164a15fbf6 58 float Cg_down = 0.00;
Khanchana 1:13164a15fbf6 59 float Cg_up = 0;
worasuchad 2:18835f8732ad 60 float Cg = 0.00, CgPrevious = 0.00;
worasuchad 3:5e867483469e 61
worasuchad 3:5e867483469e 62 // servo motor param
Khanchana 0:43d21d5145d3 63 float pos_down_start = 1400.00;
Khanchana 0:43d21d5145d3 64 float pos_up_start = 1000.00;
Khanchana 0:43d21d5145d3 65 float down_degree = 90.00;
Khanchana 0:43d21d5145d3 66 float up_degree = 45.00;
worasuchad 5:08334c6a42ca 67 float stepmin = 1.5;
Khanchana 0:43d21d5145d3 68
worasuchad 5:08334c6a42ca 69 // servo left side
Khanchana 0:43d21d5145d3 70 float pos_down_left = 1400.00;
Khanchana 0:43d21d5145d3 71 float pos_up_left = 1000.00;
Khanchana 0:43d21d5145d3 72 float pos_down_end_left;
Khanchana 0:43d21d5145d3 73 float pos_up_end_left;
Khanchana 0:43d21d5145d3 74 float state_count_left = 1;
Khanchana 0:43d21d5145d3 75 float round_count_left = 1;
Khanchana 0:43d21d5145d3 76 float step_down_left;
Khanchana 0:43d21d5145d3 77 float step_up_left;
worasuchad 5:08334c6a42ca 78 // servo right side
Khanchana 0:43d21d5145d3 79 float pos_down_right = 1400.00;
Khanchana 0:43d21d5145d3 80 float pos_up_right = 1000.00;
Khanchana 0:43d21d5145d3 81 float pos_down_end_right;
Khanchana 0:43d21d5145d3 82 float pos_up_end_right;
Khanchana 0:43d21d5145d3 83 float state_count_right = 1;
Khanchana 0:43d21d5145d3 84 float round_count_right = 1;
Khanchana 0:43d21d5145d3 85 float step_up_right;
Khanchana 0:43d21d5145d3 86 float step_down_right;
Khanchana 0:43d21d5145d3 87
worasuchad 5:08334c6a42ca 88 ////// debug param //////
worasuchad 5:08334c6a42ca 89 // print state gait
worasuchad 5:08334c6a42ca 90 int stateGaitOne = 0, stateGaitTwo = 0, stateGaitThree = 0, stateGaitFour = 0;
worasuchad 2:18835f8732ad 91 ///////////////////////// main ////////////////////////////
worasuchad 2:18835f8732ad 92 //////////////////////////////////////////////////////////////////
worasuchad 2:18835f8732ad 93 int main()
worasuchad 2:18835f8732ad 94 {
worasuchad 2:18835f8732ad 95 pc.baud(115200);
worasuchad 3:5e867483469e 96 timer1.start(); // start timer counting
worasuchad 3:5e867483469e 97 attitude_setup(); // IMU setup
worasuchad 3:5e867483469e 98 //thread1.start(IMU); // IMU thread start
worasuchad 3:5e867483469e 99 pc.printf(" Please press! '1' to start..\n\r");
worasuchad 2:18835f8732ad 100 if (pc.getc() == '1')
worasuchad 3:5e867483469e 101 {
worasuchad 3:5e867483469e 102 thread1.start(IMU); // IMU thread start
worasuchad 3:5e867483469e 103 //thread2.start(servo); // servo thread start
worasuchad 3:5e867483469e 104 }
worasuchad 3:5e867483469e 105 }
worasuchad 3:5e867483469e 106
worasuchad 3:5e867483469e 107 ///////////////////////// IMU /////////////////////////////
worasuchad 3:5e867483469e 108 //////////////////////////////////////////////////////////////////
worasuchad 3:5e867483469e 109 void IMU()
worasuchad 3:5e867483469e 110 {
worasuchad 3:5e867483469e 111 int i;
worasuchad 5:08334c6a42ca 112 float ArrayOfRoll[10], ArrayOfPitch[10], ArrayOfYaw[10];
worasuchad 5:08334c6a42ca 113 float SDOfRoll, SDOfPitch, SDOfYaw, FirstOfRoll, FirstOfPitch, FirstOfYaw;
worasuchad 5:08334c6a42ca 114 float DiffOfRP = 0.0f;
worasuchad 3:5e867483469e 115 /* pc.printf("roll\t");
worasuchad 3:5e867483469e 116 pc.printf("Si\t");
worasuchad 3:5e867483469e 117 pc.printf("Cg\t");
worasuchad 3:5e867483469e 118 pc.printf("down\t");
worasuchad 3:5e867483469e 119 pc.printf("up\n"); */
worasuchad 3:5e867483469e 120 while(1)
worasuchad 3:5e867483469e 121 {
worasuchad 5:08334c6a42ca 122 if (timer1.read_us() >= 500) // read time in 0.5 ms
worasuchad 3:5e867483469e 123 {
worasuchad 3:5e867483469e 124 attitude_get();
worasuchad 3:5e867483469e 125
worasuchad 3:5e867483469e 126 //pc.printf(" %f \t", ax*10 );
worasuchad 3:5e867483469e 127 //pc.printf(" %f \t", ay*10 );
worasuchad 3:5e867483469e 128 //pc.printf(" %f \t", az*10-10); //cm/s*s
worasuchad 3:5e867483469e 129
worasuchad 3:5e867483469e 130 //pc.printf("%f\t %f\t %f\n\r", roll, pitch, yaw);
worasuchad 3:5e867483469e 131 //pc.printf("up\t");
worasuchad 3:5e867483469e 132 //pc.printf("%f \t",up_degree);
worasuchad 3:5e867483469e 133 //pc.printf("%f\t",Cg);
worasuchad 3:5e867483469e 134 //pc.printf("down\t");
worasuchad 3:5e867483469e 135 //pc.printf("%f \t",down_degree);
worasuchad 3:5e867483469e 136 //pc.printf("%f\n",Cg);
worasuchad 3:5e867483469e 137
worasuchad 3:5e867483469e 138 ////////////////////////// Signal Pre-Process every 10 ms //////////////////////
worasuchad 3:5e867483469e 139 ////////////////////////////////////////////////////////////////////////////////
worasuchad 3:5e867483469e 140 if(i < 10)
worasuchad 3:5e867483469e 141 {
worasuchad 3:5e867483469e 142 ArrayOfRoll[i] = roll;
worasuchad 5:08334c6a42ca 143 ArrayOfPitch[i] = pitch;
worasuchad 3:5e867483469e 144 ArrayOfYaw[i] = yaw;
worasuchad 3:5e867483469e 145 //pc.printf("i = %i ,ArrayOfRoll = %.2f, roll= %.2f\n\r",i, ArrayOfRoll[i], roll);
worasuchad 3:5e867483469e 146 i++;
worasuchad 3:5e867483469e 147 }
worasuchad 5:08334c6a42ca 148 else // every 5 ms
worasuchad 3:5e867483469e 149 {
worasuchad 5:08334c6a42ca 150 ////// print state of gait /////
worasuchad 5:08334c6a42ca 151 //printStateGait();
worasuchad 5:08334c6a42ca 152
worasuchad 5:08334c6a42ca 153 // the accleration
worasuchad 5:08334c6a42ca 154 //pc.printf("%.3f\t\t", gx*PI/180.0f);
worasuchad 5:08334c6a42ca 155 //pc.printf("%.3f\t\t", gy*PI/180.0f);
worasuchad 5:08334c6a42ca 156 //pc.printf("%.3f\t\t", gz*PI/180.0f);
worasuchad 5:08334c6a42ca 157
worasuchad 5:08334c6a42ca 158 // the gyro value
worasuchad 5:08334c6a42ca 159 //pc.printf("%.3f\t\t", ax * 9.81f ); // convert g to m/s^2
worasuchad 5:08334c6a42ca 160 //pc.printf("%.3f\t\t", ay * 9.81f); // convert g to m/s^2
worasuchad 5:08334c6a42ca 161 //pc.printf("%.3f\n\r", ( az - 1 ) * 9.81f); // m/s^2 and 1g for eliminating earth gravity
worasuchad 5:08334c6a42ca 162
worasuchad 5:08334c6a42ca 163 // diff roll pitch
worasuchad 5:08334c6a42ca 164 if(state_count_left == 4 and state_count_right == 4 )
worasuchad 5:08334c6a42ca 165 {
worasuchad 5:08334c6a42ca 166
worasuchad 5:08334c6a42ca 167 DiffOfRP = sqrt( pow(roll,2) + pow(pitch,2) );
worasuchad 5:08334c6a42ca 168 pc.printf("%.3f\n\r", DiffOfRP);
worasuchad 5:08334c6a42ca 169 DiffOfRP = 0.0f;
worasuchad 5:08334c6a42ca 170 }
worasuchad 3:5e867483469e 171 //////////// roll //////////////
worasuchad 3:5e867483469e 172 SDOfRoll = calculateSD(ArrayOfRoll);
worasuchad 5:08334c6a42ca 173 //pc.printf("%.3f\t\t", SDOfRoll);
worasuchad 5:08334c6a42ca 174 //pc.printf("%.3f\t\t", roll);
worasuchad 5:08334c6a42ca 175
worasuchad 3:5e867483469e 176 //pc.printf("HC: %.3f\n\r", HormoneCon(SDOfRoll));
worasuchad 5:08334c6a42ca 177 //HormoneCon(SDOfRoll);
worasuchad 5:08334c6a42ca 178
worasuchad 5:08334c6a42ca 179 //////////// pitch ///////////////
worasuchad 5:08334c6a42ca 180 SDOfPitch = calculateSD(ArrayOfPitch);
worasuchad 5:08334c6a42ca 181 //pc.printf("%.3f\t\t", SDOfPitch);
worasuchad 5:08334c6a42ca 182 //pc.printf("%.3f\t\t", pitch);
worasuchad 3:5e867483469e 183
worasuchad 3:5e867483469e 184 //////////// yaw ///////////////
worasuchad 3:5e867483469e 185 SDOfYaw = calculateSD(ArrayOfYaw);
worasuchad 5:08334c6a42ca 186 //pc.printf("%.3f\t\t", SDOfYaw);
worasuchad 5:08334c6a42ca 187 //pc.printf("%.3f\n\r", yaw);
worasuchad 3:5e867483469e 188
worasuchad 5:08334c6a42ca 189 if(checkIMUFirst(SDOfRoll, SDOfPitch, SDOfYaw)) // only one time for comming
worasuchad 3:5e867483469e 190 {
worasuchad 5:08334c6a42ca 191 FirstOfRoll = calculateMean(ArrayOfRoll);
worasuchad 5:08334c6a42ca 192 FirstOfPitch = calculateMean(ArrayOfPitch);
worasuchad 5:08334c6a42ca 193 FirstOfYaw = calculateMean(ArrayOfYaw);
worasuchad 5:08334c6a42ca 194 pc.printf("FirstOfRoll: %.3f, FirstOfPitch: %.3f, FirstOfYaw: %.3f\n\r", FirstOfRoll, FirstOfPitch, FirstOfYaw);
worasuchad 3:5e867483469e 195 thread2.start(servo); // Servo Thread
worasuchad 5:08334c6a42ca 196 pc.printf("\n\r:::::::::: IMU was stable ::::::::::\n\r");
worasuchad 3:5e867483469e 197 }
worasuchad 5:08334c6a42ca 198
worasuchad 5:08334c6a42ca 199 //pc.printf("directOfRobot: %.3f\n\r", directOfRobot);
worasuchad 3:5e867483469e 200 // func find diff beween directOfRobot and MeanOfYaw
worasuchad 4:ec7e68b84f2b 201 //pc.printf("Diff from direction: %.3f\n\r", abs(directOfRobot - MeanOfYaw));
worasuchad 5:08334c6a42ca 202
worasuchad 5:08334c6a42ca 203 /*pc.printf("Cg_down: %f Cg_up: %f \t", Cg_down, Cg_up);
worasuchad 5:08334c6a42ca 204 pc.printf("down: \t");
worasuchad 5:08334c6a42ca 205 pc.printf("%f \t",down_degree);
worasuchad 5:08334c6a42ca 206 pc.printf("up: \t");
worasuchad 5:08334c6a42ca 207 pc.printf("%f \n\r",up_degree);*/
worasuchad 5:08334c6a42ca 208
worasuchad 3:5e867483469e 209 // reset iteration
worasuchad 3:5e867483469e 210 i = 0;
worasuchad 3:5e867483469e 211 }
worasuchad 3:5e867483469e 212
worasuchad 3:5e867483469e 213 timer1.reset(); // reset timer
worasuchad 3:5e867483469e 214 }
worasuchad 2:18835f8732ad 215 }
worasuchad 3:5e867483469e 216 }
worasuchad 3:5e867483469e 217
worasuchad 3:5e867483469e 218 ///////////////////////// servo ///////////////////////////
worasuchad 3:5e867483469e 219 //////////////////////////////////////////////////////////////////
worasuchad 3:5e867483469e 220 void servo()
worasuchad 2:18835f8732ad 221 {
worasuchad 5:08334c6a42ca 222 /*Servo1.Enable(1000,20000);
worasuchad 3:5e867483469e 223 Servo2.Enable(1000,20000);
worasuchad 3:5e867483469e 224 Servo3.Enable(1000,20000);
worasuchad 5:08334c6a42ca 225 Servo4.Enable(1000,20000);*/
worasuchad 3:5e867483469e 226
worasuchad 5:08334c6a42ca 227 pc.printf("\n\r Servo Start Ja!!! \n\r");
worasuchad 3:5e867483469e 228 timerwalk.start(); // start timer counting
worasuchad 5:08334c6a42ca 229
worasuchad 3:5e867483469e 230 while(1)
worasuchad 3:5e867483469e 231 {
worasuchad 3:5e867483469e 232 receive_hormone();
worasuchad 3:5e867483469e 233 cal_step_down(); // return "step_down_right" and "step_down_left"
worasuchad 3:5e867483469e 234 cal_step_up(); // return "step_up_right" and "step_up_left"
worasuchad 3:5e867483469e 235 servo_Left(); // control left lag
worasuchad 3:5e867483469e 236 servo_Right(); // control right leg
worasuchad 3:5e867483469e 237
worasuchad 3:5e867483469e 238 // fin for walking
worasuchad 3:5e867483469e 239 if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round)
worasuchad 3:5e867483469e 240 {
worasuchad 3:5e867483469e 241 thread1.terminate();
worasuchad 3:5e867483469e 242 pc.printf("Finish! \t");
worasuchad 3:5e867483469e 243 walking_time = timerwalk.read_ms();
worasuchad 3:5e867483469e 244 pc.printf("Walking time = %d ms\n\r", walking_time);
worasuchad 3:5e867483469e 245 break;
worasuchad 3:5e867483469e 246 }
worasuchad 3:5e867483469e 247 }
worasuchad 3:5e867483469e 248 }
Khanchana 0:43d21d5145d3 249
worasuchad 2:18835f8732ad 250 ///////////////////////// receive_hormone /////////////////
worasuchad 2:18835f8732ad 251 //////////////////////////////////////////////////////////////////
worasuchad 2:18835f8732ad 252 void receive_hormone()
worasuchad 2:18835f8732ad 253 {
worasuchad 2:18835f8732ad 254 //down_degree = 90.00f*(1.00f-(0.06f*Cg_down));
worasuchad 5:08334c6a42ca 255 down_degree = 85.00;
worasuchad 5:08334c6a42ca 256 /*pc.printf("Cg_down: %f Cg_up: %f \t", Cg_down, Cg_up);
worasuchad 5:08334c6a42ca 257 pc.printf("down: \t");
worasuchad 5:08334c6a42ca 258 pc.printf("%f \t",down_degree);*/
worasuchad 5:08334c6a42ca 259 //pc.printf("%f\t",Cg);
worasuchad 2:18835f8732ad 260 //up_degree = 45.00f*(1.00f+(0.7f*Cg_up));
worasuchad 2:18835f8732ad 261 up_degree = 45.00;
worasuchad 5:08334c6a42ca 262 /*pc.printf("up: \t");
worasuchad 5:08334c6a42ca 263 pc.printf("%f \n\r",up_degree);*/
worasuchad 5:08334c6a42ca 264 //pc.printf("%f\n",Cg);
worasuchad 2:18835f8732ad 265 if(down_degree < 85)
worasuchad 2:18835f8732ad 266 {
Khanchana 1:13164a15fbf6 267 down_degree = 85;
worasuchad 2:18835f8732ad 268 if(up_degree > 75)
worasuchad 2:18835f8732ad 269 {
worasuchad 2:18835f8732ad 270 up_degree = 75;
Khanchana 0:43d21d5145d3 271 }
worasuchad 5:08334c6a42ca 272 }
Khanchana 0:43d21d5145d3 273 }
Khanchana 0:43d21d5145d3 274
worasuchad 2:18835f8732ad 275 ///////////////////////// cal_step_down ///////////////////
worasuchad 2:18835f8732ad 276 //////////////////////////////////////////////////////////////////
worasuchad 2:18835f8732ad 277 void cal_step_down()
worasuchad 2:18835f8732ad 278 {
Khanchana 0:43d21d5145d3 279 //pc.printf("down");
Khanchana 0:43d21d5145d3 280 //pc.printf("%f \n",down_degree);
worasuchad 2:18835f8732ad 281 pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); // get degree for hormone receiver about down_degree ~ 90*,
worasuchad 5:08334c6a42ca 282 pos_down_end_right = (1060.00 + ((700.00/90.00)*(down_degree))); // so both pos_down_end_left and pos_down_end_right are around 1700
worasuchad 2:18835f8732ad 283 if (pos_down_end_right > pos_down_end_left)
worasuchad 2:18835f8732ad 284 {
worasuchad 2:18835f8732ad 285 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 286 step_down_left = stepmin;
worasuchad 2:18835f8732ad 287 }
worasuchad 2:18835f8732ad 288 else if (pos_down_end_right < pos_down_end_left)
worasuchad 2:18835f8732ad 289 {
Khanchana 0:43d21d5145d3 290 step_down_right = stepmin;
Khanchana 0:43d21d5145d3 291 step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
worasuchad 2:18835f8732ad 292 }
worasuchad 2:18835f8732ad 293 else // pos_down_end_right == pos_down_end_left
worasuchad 2:18835f8732ad 294 {
Khanchana 0:43d21d5145d3 295 step_down_right = stepmin;
Khanchana 0:43d21d5145d3 296 step_down_left = stepmin;
Khanchana 0:43d21d5145d3 297 }
worasuchad 3:5e867483469e 298 /*pc.printf("pos_down_right: ");
worasuchad 3:5e867483469e 299 pc.printf("%f\t\t",pos_down_end_right);
worasuchad 3:5e867483469e 300 pc.printf("pos_down_left: ");
worasuchad 3:5e867483469e 301 pc.printf("%f\t\t",pos_down_end_left);
worasuchad 3:5e867483469e 302 pc.printf("step_down_right: ");
worasuchad 3:5e867483469e 303 pc.printf("%f\t\t",step_down_right);
worasuchad 3:5e867483469e 304 pc.printf("step_down_left: ");
worasuchad 3:5e867483469e 305 pc.printf("%f\n\r",step_down_left);*/
Khanchana 0:43d21d5145d3 306 }
worasuchad 2:18835f8732ad 307
worasuchad 2:18835f8732ad 308 ///////////////////////// cal_step_up /////////////////////
worasuchad 2:18835f8732ad 309 //////////////////////////////////////////////////////////////////
worasuchad 2:18835f8732ad 310 void cal_step_up()
worasuchad 2:18835f8732ad 311 {
Khanchana 0:43d21d5145d3 312 //pc.printf("up");
Khanchana 0:43d21d5145d3 313 //pc.printf("%f \n",up_degree);
worasuchad 2:18835f8732ad 314 pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); // get degree for hormone receiver about up_degree ~ 45*,
worasuchad 2:18835f8732ad 315 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 316 if (pos_up_end_right > pos_up_end_left)
worasuchad 2:18835f8732ad 317 {
worasuchad 2:18835f8732ad 318 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 319 step_up_left = stepmin;
worasuchad 2:18835f8732ad 320 }
worasuchad 2:18835f8732ad 321 else if (pos_up_end_right < pos_up_end_left)
worasuchad 2:18835f8732ad 322 {
Khanchana 0:43d21d5145d3 323 step_up_right = stepmin;
Khanchana 0:43d21d5145d3 324 step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
worasuchad 2:18835f8732ad 325 }
worasuchad 2:18835f8732ad 326 else // step_up_right == step_up_left
worasuchad 2:18835f8732ad 327 {
Khanchana 0:43d21d5145d3 328 step_up_right = stepmin;
Khanchana 0:43d21d5145d3 329 step_up_left = stepmin;
Khanchana 0:43d21d5145d3 330 }
worasuchad 3:5e867483469e 331 /*pc.printf("pos_up_right: ");
worasuchad 3:5e867483469e 332 pc.printf("%f\t\t",pos_up_end_right);
worasuchad 3:5e867483469e 333 pc.printf("pos_up_left: ");
worasuchad 3:5e867483469e 334 pc.printf("%f\t\t",pos_up_end_left);
worasuchad 3:5e867483469e 335 pc.printf("step_up_right: ");
worasuchad 3:5e867483469e 336 pc.printf("%f\t\t",step_up_right);;
worasuchad 3:5e867483469e 337 pc.printf("step_up_left: ");
worasuchad 3:5e867483469e 338 pc.printf("%f\n\r",step_up_left);*/
Khanchana 0:43d21d5145d3 339 }
worasuchad 2:18835f8732ad 340
worasuchad 5:08334c6a42ca 341 ///////////////////// Print State Gait //////////////////////
worasuchad 5:08334c6a42ca 342 //////////////////////////////////////////////////////////////////
worasuchad 5:08334c6a42ca 343 void printStateGait()
worasuchad 5:08334c6a42ca 344 {
worasuchad 5:08334c6a42ca 345 if(state_count_left == 1 and state_count_right == 1 and stateGaitOne == 0)
worasuchad 5:08334c6a42ca 346 {
worasuchad 5:08334c6a42ca 347 pc.printf("\n\r State Gait 1 \n\r");
worasuchad 5:08334c6a42ca 348 stateGaitOne = 1; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 0;
worasuchad 5:08334c6a42ca 349 }
worasuchad 5:08334c6a42ca 350 else if(state_count_left == 2 and state_count_right == 2 and stateGaitTwo == 0)
worasuchad 5:08334c6a42ca 351 {
worasuchad 5:08334c6a42ca 352 pc.printf("\n\r State Gait 2 \n\r");
worasuchad 5:08334c6a42ca 353 stateGaitOne = 0; stateGaitTwo = 1; stateGaitThree = 0; stateGaitFour = 0;
worasuchad 5:08334c6a42ca 354 }
worasuchad 5:08334c6a42ca 355 else if(state_count_left == 3 and state_count_right == 3 and stateGaitThree == 0)
worasuchad 5:08334c6a42ca 356 {
worasuchad 5:08334c6a42ca 357 pc.printf("\n\r State Gait 3 \n\r");
worasuchad 5:08334c6a42ca 358 stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 1; stateGaitFour = 0;
worasuchad 5:08334c6a42ca 359 }
worasuchad 5:08334c6a42ca 360 else if(state_count_left == 4 and state_count_right == 4 and stateGaitFour == 0)
worasuchad 5:08334c6a42ca 361 {
worasuchad 5:08334c6a42ca 362 pc.printf("\n\r State Gait 4 \n\r");
worasuchad 5:08334c6a42ca 363 stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 1;
worasuchad 5:08334c6a42ca 364 }
worasuchad 5:08334c6a42ca 365 }
worasuchad 5:08334c6a42ca 366
worasuchad 3:5e867483469e 367 ///////////////////////// servo_Left //////////////////////
worasuchad 3:5e867483469e 368 //////////////////////////////////////////////////////////////////
worasuchad 3:5e867483469e 369 void servo_Left()
worasuchad 2:18835f8732ad 370 {
worasuchad 3:5e867483469e 371 if(state_count_left == 1)
worasuchad 2:18835f8732ad 372 {
worasuchad 2:18835f8732ad 373 Servo1.SetPosition(pos_down_left); // pos_down_left = 1400.00
worasuchad 2:18835f8732ad 374 wait(waittime); // 0.001 ms
worasuchad 3:5e867483469e 375 pos_down_left += step_down_left;
worasuchad 2:18835f8732ad 376 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 377 {
Khanchana 0:43d21d5145d3 378 state_count_left = 2;
Khanchana 0:43d21d5145d3 379 }
Khanchana 0:43d21d5145d3 380 /*pc.printf("LAD");
Khanchana 0:43d21d5145d3 381 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 382 pc.printf("LAP");
Khanchana 0:43d21d5145d3 383 pc.printf("%f\n",pos_up_left);*/
worasuchad 2:18835f8732ad 384 }
worasuchad 2:18835f8732ad 385 else if(state_count_left == 2)
worasuchad 2:18835f8732ad 386 {
worasuchad 2:18835f8732ad 387 Servo2.SetPosition(pos_up_left); // pos_up_left = 1000.00
Khanchana 0:43d21d5145d3 388 wait(waittime);
worasuchad 3:5e867483469e 389 pos_up_left += step_up_left;
worasuchad 2:18835f8732ad 390 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 391 {
Khanchana 0:43d21d5145d3 392 state_count_left = 3;
Khanchana 0:43d21d5145d3 393 }
Khanchana 0:43d21d5145d3 394 /*pc.printf("LBD");
Khanchana 0:43d21d5145d3 395 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 396 pc.printf("LBP");
Khanchana 0:43d21d5145d3 397 pc.printf("%f\n",pos_up_left);*/
worasuchad 2:18835f8732ad 398 }
worasuchad 2:18835f8732ad 399 else if(state_count_left == 3)
worasuchad 2:18835f8732ad 400 {
Khanchana 0:43d21d5145d3 401 Servo1.SetPosition(pos_down_left);
Khanchana 0:43d21d5145d3 402 wait(waittime);
worasuchad 3:5e867483469e 403 pos_down_left -= step_down_left;
worasuchad 2:18835f8732ad 404 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left)
worasuchad 2:18835f8732ad 405 {
Khanchana 0:43d21d5145d3 406 state_count_left = 4;
Khanchana 0:43d21d5145d3 407 }
Khanchana 0:43d21d5145d3 408 /*pc.printf("LCD");
Khanchana 0:43d21d5145d3 409 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 410 pc.printf("LCP");
Khanchana 0:43d21d5145d3 411 pc.printf("%f\n",pos_up_left);*/
worasuchad 2:18835f8732ad 412 }
worasuchad 2:18835f8732ad 413 else if(state_count_left == 4)
worasuchad 2:18835f8732ad 414 {
Khanchana 0:43d21d5145d3 415 Servo2.SetPosition(pos_up_left);
Khanchana 0:43d21d5145d3 416 wait(waittime);
worasuchad 3:5e867483469e 417 pos_up_left -= step_up_left;
worasuchad 2:18835f8732ad 418 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left)
worasuchad 2:18835f8732ad 419 {
Khanchana 0:43d21d5145d3 420 state_count_left = 0;
Khanchana 0:43d21d5145d3 421 }
Khanchana 0:43d21d5145d3 422 /*pc.printf("LDD");
Khanchana 0:43d21d5145d3 423 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 424 pc.printf("LDP");
Khanchana 0:43d21d5145d3 425 pc.printf("%f\n",pos_up_left);*/
worasuchad 2:18835f8732ad 426 }
worasuchad 2:18835f8732ad 427 else if (state_count_left == 0 and round_count_left < round)
worasuchad 2:18835f8732ad 428 {
worasuchad 3:5e867483469e 429 round_count_left++;
Khanchana 0:43d21d5145d3 430 state_count_left = 1;
Khanchana 0:43d21d5145d3 431 pos_down_left = pos_down_start;
Khanchana 0:43d21d5145d3 432 pos_up_left = pos_up_start;
worasuchad 2:18835f8732ad 433 }
Khanchana 0:43d21d5145d3 434 }
worasuchad 2:18835f8732ad 435
worasuchad 2:18835f8732ad 436 ///////////////////////// servo_Right /////////////////////
worasuchad 2:18835f8732ad 437 //////////////////////////////////////////////////////////////////
Khanchana 0:43d21d5145d3 438 void servo_Right()
Khanchana 0:43d21d5145d3 439 {
worasuchad 2:18835f8732ad 440 if(state_count_right == 1)
worasuchad 2:18835f8732ad 441 {
Khanchana 0:43d21d5145d3 442 Servo3.SetPosition(pos_down_right);
Khanchana 0:43d21d5145d3 443 wait(waittime);
Khanchana 0:43d21d5145d3 444 pos_down_right = pos_down_right + step_down_right;
worasuchad 2:18835f8732ad 445 if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start)
worasuchad 2:18835f8732ad 446 {
Khanchana 0:43d21d5145d3 447 state_count_right = 2;
Khanchana 0:43d21d5145d3 448 }
Khanchana 0:43d21d5145d3 449 /*pc.printf("RAD");
Khanchana 0:43d21d5145d3 450 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 451 pc.printf("RAP");
Khanchana 0:43d21d5145d3 452 pc.printf("%f\n",pos_up_right);*/
worasuchad 2:18835f8732ad 453 }
worasuchad 2:18835f8732ad 454 else if(state_count_right == 2)
worasuchad 2:18835f8732ad 455 {
Khanchana 0:43d21d5145d3 456 Servo4.SetPosition(pos_up_right);
Khanchana 0:43d21d5145d3 457 wait(waittime);
Khanchana 0:43d21d5145d3 458 pos_up_right = pos_up_right + step_up_right;
worasuchad 2:18835f8732ad 459 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 460 {
Khanchana 0:43d21d5145d3 461 state_count_right = 3;
Khanchana 0:43d21d5145d3 462 }
Khanchana 0:43d21d5145d3 463 /*pc.printf("RBD");
Khanchana 0:43d21d5145d3 464 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 465 pc.printf("RBP");
Khanchana 0:43d21d5145d3 466 pc.printf("%f\n",pos_up_right);*/
worasuchad 2:18835f8732ad 467 }
worasuchad 2:18835f8732ad 468 else if(state_count_right == 3)
worasuchad 2:18835f8732ad 469 {
Khanchana 0:43d21d5145d3 470 Servo3.SetPosition(pos_down_right);
Khanchana 0:43d21d5145d3 471 wait(waittime);
Khanchana 0:43d21d5145d3 472 pos_down_right = pos_down_right - step_down_right;
worasuchad 2:18835f8732ad 473 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right)
worasuchad 2:18835f8732ad 474 {
Khanchana 0:43d21d5145d3 475 state_count_right = 4;
Khanchana 0:43d21d5145d3 476 }
Khanchana 0:43d21d5145d3 477 /*pc.printf("RCD");
Khanchana 0:43d21d5145d3 478 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 479 pc.printf("RCP");
Khanchana 0:43d21d5145d3 480 pc.printf("%f\n",pos_up_right);*/
worasuchad 2:18835f8732ad 481 }
worasuchad 2:18835f8732ad 482 else if(state_count_right == 4)
worasuchad 2:18835f8732ad 483 {
Khanchana 0:43d21d5145d3 484 Servo4.SetPosition(pos_up_right);
worasuchad 2:18835f8732ad 485 wait(waittime);
Khanchana 0:43d21d5145d3 486 pos_up_right = pos_up_right - step_up_right;
worasuchad 2:18835f8732ad 487 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right)
worasuchad 2:18835f8732ad 488 {
Khanchana 0:43d21d5145d3 489 state_count_right = 0;
Khanchana 0:43d21d5145d3 490 }
Khanchana 0:43d21d5145d3 491 /*pc.printf("RDD");
Khanchana 0:43d21d5145d3 492 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 493 pc.printf("RDP");
Khanchana 0:43d21d5145d3 494 pc.printf("%f\n",pos_up_right);*/
worasuchad 2:18835f8732ad 495 }
worasuchad 2:18835f8732ad 496 else if (state_count_right == 0 and round_count_right < round)
worasuchad 2:18835f8732ad 497 {
Khanchana 0:43d21d5145d3 498 round_count_right = round_count_right+1;
Khanchana 0:43d21d5145d3 499 state_count_right = 1;
Khanchana 0:43d21d5145d3 500 pos_down_right = pos_down_start;
Khanchana 0:43d21d5145d3 501 pos_up_right = pos_up_start;
Khanchana 0:43d21d5145d3 502 }
Khanchana 0:43d21d5145d3 503 }
Khanchana 0:43d21d5145d3 504
worasuchad 2:18835f8732ad 505 ///////////////// Hormone Concentration /////////////////////
worasuchad 2:18835f8732ad 506 //////////////////////////////////////////////////////////////////
worasuchad 2:18835f8732ad 507 float HormoneCon(float SiPreProcess)
worasuchad 2:18835f8732ad 508 {
worasuchad 2:18835f8732ad 509 float CgTemp;
worasuchad 2:18835f8732ad 510
worasuchad 2:18835f8732ad 511 //pc.printf("SiPreProcess: %.3f\t", SiPreProcess);
worasuchad 2:18835f8732ad 512 //pc.printf("CgPrevious: %.3f\t", CgPrevious);
worasuchad 2:18835f8732ad 513
worasuchad 2:18835f8732ad 514 ////// hormone gland //////
worasuchad 2:18835f8732ad 515 CgTemp = (0.8f*SiPreProcess) + (0.5f*CgPrevious);
worasuchad 5:08334c6a42ca 516 //pc.printf("CgTemp: %.3f\t\t", CgTemp);
worasuchad 2:18835f8732ad 517 Cg = 1/( 1+exp(-CgTemp) ); // used sigmoid func for calculating Cg which much have value between 0 - 1
worasuchad 5:08334c6a42ca 518 //pc.printf("Cg: %.3f\n\r", Cg);
worasuchad 2:18835f8732ad 519 ///////////////////////////
worasuchad 2:18835f8732ad 520
worasuchad 2:18835f8732ad 521 //pc.printf("First Term: %.3f\t", (float)(0.8f*SiPreProcess));
worasuchad 2:18835f8732ad 522 //pc.printf("Secound Term: %.3f\t", (float)(0.5f*CgPrevious));
worasuchad 2:18835f8732ad 523 CgPrevious = Cg;
worasuchad 2:18835f8732ad 524 //*********//
worasuchad 2:18835f8732ad 525 Cg_down = Cg;
worasuchad 2:18835f8732ad 526 Cg_up = Cg;
worasuchad 2:18835f8732ad 527
worasuchad 2:18835f8732ad 528 return Cg;
Khanchana 0:43d21d5145d3 529 }
Khanchana 0:43d21d5145d3 530
worasuchad 2:18835f8732ad 531 ///////////////////////// calculateSD /////////////////////
worasuchad 2:18835f8732ad 532 //////////////////////////////////////////////////////////////////
worasuchad 2:18835f8732ad 533 float calculateSD(float sdData[])
worasuchad 2:18835f8732ad 534 {
worasuchad 2:18835f8732ad 535 float sum = 0.0, mean, standardDeviation = 0.0;
worasuchad 2:18835f8732ad 536 int i;
worasuchad 2:18835f8732ad 537
worasuchad 3:5e867483469e 538 for(i = 0; i < 10 ; ++i)
worasuchad 2:18835f8732ad 539 {
worasuchad 2:18835f8732ad 540 sum += sdData[i];
worasuchad 2:18835f8732ad 541 //pc.printf("sum = %.2f \n\r",sum);
worasuchad 2:18835f8732ad 542 }
worasuchad 2:18835f8732ad 543 mean = sum/10;
worasuchad 2:18835f8732ad 544 //pc.printf("mean = %.2f \n\r",mean);
worasuchad 2:18835f8732ad 545
worasuchad 3:5e867483469e 546 for(i = 0; i < 10; ++i)
worasuchad 2:18835f8732ad 547 {
worasuchad 2:18835f8732ad 548 standardDeviation += pow(sdData[i] - mean, 2);
worasuchad 2:18835f8732ad 549 //pc.printf("standardDeviation = %.2f \n\r",standardDeviation);
Khanchana 0:43d21d5145d3 550 }
worasuchad 2:18835f8732ad 551 return sqrt(standardDeviation / 10);
worasuchad 3:5e867483469e 552 }
worasuchad 3:5e867483469e 553
worasuchad 3:5e867483469e 554 ///////////////////////// calculateMean ///////////////////
worasuchad 3:5e867483469e 555 //////////////////////////////////////////////////////////////////
worasuchad 3:5e867483469e 556 float calculateMean(float meanData[])
worasuchad 3:5e867483469e 557 {
worasuchad 3:5e867483469e 558 float sum = 0.0, mean;
worasuchad 3:5e867483469e 559 int i;
worasuchad 3:5e867483469e 560
worasuchad 3:5e867483469e 561 for(i = 0; i < 10 ; ++i)
worasuchad 3:5e867483469e 562 {
worasuchad 3:5e867483469e 563 sum += meanData[i];
worasuchad 3:5e867483469e 564 //pc.printf("sum = %.2f \n\r",sum);
worasuchad 3:5e867483469e 565 }
worasuchad 3:5e867483469e 566 mean = sum/10;
worasuchad 3:5e867483469e 567 //pc.printf("mean = %.2f \n\r",mean);
worasuchad 3:5e867483469e 568 return mean;
worasuchad 3:5e867483469e 569 }
worasuchad 3:5e867483469e 570
worasuchad 5:08334c6a42ca 571 ///////////////////////// check IMU first //////////////////
worasuchad 3:5e867483469e 572 //////////////////////////////////////////////////////////////////
worasuchad 5:08334c6a42ca 573 bool checkIMUFirst(float SDOfRoll, float SDOfPitch, float SDOfYaw)
worasuchad 3:5e867483469e 574 {
worasuchad 5:08334c6a42ca 575 if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 0)
worasuchad 5:08334c6a42ca 576 {
worasuchad 5:08334c6a42ca 577 servoFirstState();
worasuchad 5:08334c6a42ca 578 initCheck++;
worasuchad 5:08334c6a42ca 579 }
worasuchad 5:08334c6a42ca 580 else if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 1)
worasuchad 3:5e867483469e 581 {
worasuchad 5:08334c6a42ca 582 initCheck++;
worasuchad 5:08334c6a42ca 583 return 1;
worasuchad 5:08334c6a42ca 584 }
worasuchad 3:5e867483469e 585 return 0;
worasuchad 5:08334c6a42ca 586 }
worasuchad 5:08334c6a42ca 587
worasuchad 5:08334c6a42ca 588 ///////////////////// servo First State /////////////////////
worasuchad 5:08334c6a42ca 589 //////////////////////////////////////////////////////////////////
worasuchad 5:08334c6a42ca 590 void servoFirstState()
worasuchad 5:08334c6a42ca 591 {
worasuchad 5:08334c6a42ca 592 Servo1.Enable(1000,20000);
worasuchad 5:08334c6a42ca 593 Servo2.Enable(1000,20000);
worasuchad 5:08334c6a42ca 594 Servo3.Enable(1000,20000);
worasuchad 5:08334c6a42ca 595 Servo4.Enable(1000,20000);
worasuchad 5:08334c6a42ca 596 pc.printf("Servo First State\n\r");
worasuchad 5:08334c6a42ca 597 pc.printf("pos_down_left: %.3f\t pos_up_left: %.3f\t pos_down_right: %.3f\t pos_up_right: %.3f\n\r", pos_down_left, pos_up_left, pos_down_right, pos_up_right);
worasuchad 5:08334c6a42ca 598 Servo1.SetPosition(pos_down_left);
worasuchad 5:08334c6a42ca 599 Servo2.SetPosition(pos_up_left);
worasuchad 5:08334c6a42ca 600 Servo3.SetPosition(pos_down_right);
worasuchad 5:08334c6a42ca 601 Servo4.SetPosition(pos_up_right);
worasuchad 5:08334c6a42ca 602
Khanchana 0:43d21d5145d3 603 }