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@6:8ae55e1f7e76, 2018-07-20 (annotated)

Committer:
worasuchad
Date:
Fri Jul 20 17:44:07 2018 +0000
Revision:
6:8ae55e1f7e76
Parent:
5:08334c6a42ca
Child:
7:ca887fdc5388
v03

Who changed what in which revision?

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