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@2:18835f8732ad, 2018-06-20 (annotated)

Committer:
worasuchad
Date:
Wed Jun 20 04:59:45 2018 +0000
Revision:
2:18835f8732ad
Parent:
1:13164a15fbf6
Child:
3:5e867483469e
Modify IMU thread ;

Who changed what in which revision?

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