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@13:38dba9cfa29d, 2018-09-02 (annotated)

Committer:
worasuchad
Date:
Sun Sep 02 04:43:00 2018 +0000
Revision:
13:38dba9cfa29d
Parent:
12:a9d894e37936 
fixing issue lib had missed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
worasuchad 2:18835f8732ad 1
worasuchad 8:865535fcf917 2 /***************************< File comment >***************************/
worasuchad 8:865535fcf917 3 /* project: TurtleBot Project */
worasuchad 8:865535fcf917 4 /* project description : - */
worasuchad 8:865535fcf917 5 /*--------------------------------------------------------------------*/
worasuchad 12:a9d894e37936 6 /* version : 0.9 */
worasuchad 8:865535fcf917 7 /* board : NUCLEO-F411RE */
worasuchad 8:865535fcf917 8 /* detail : two hg is added */
worasuchad 8:865535fcf917 9 /*--------------------------------------------------------------------*/
worasuchad 12:a9d894e37936 10 /* create : 19/08/2018 */
worasuchad 8:865535fcf917 11 /* programmer : Worasuchad Haomachai */
worasuchad 8:865535fcf917 12 /**********************************************************************/
worasuchad 8:865535fcf917 13
worasuchad 8:865535fcf917 14 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 15 /* Include file */
worasuchad 8:865535fcf917 16 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 17 #include <stdlib.h>
Khanchana 0:43d21d5145d3 18 #include "mbed.h"
Khanchana 0:43d21d5145d3 19 #include "Servo.h"
Khanchana 0:43d21d5145d3 20 #include "rtos.h"
Khanchana 0:43d21d5145d3 21 #include "attitude.h"
Khanchana 1:13164a15fbf6 22 #include "math.h"
worasuchad 8:865535fcf917 23 #include "calculator.h"
worasuchad 8:865535fcf917 24 #include "hormone.h"
worasuchad 8:865535fcf917 25 #include "PowerMon.h"
Khanchana 0:43d21d5145d3 26
worasuchad 6:8ae55e1f7e76 27 Serial pc(USBTX, USBRX); // Serial Port
Khanchana 0:43d21d5145d3 28 Timer timer1;
Khanchana 0:43d21d5145d3 29 Thread thread1;
Khanchana 0:43d21d5145d3 30 Thread thread2;
worasuchad 11:8548536c3f11 31 Thread thread3;
Khanchana 0:43d21d5145d3 32
worasuchad 6:8ae55e1f7e76 33 Servo Servo1(D4); // Servo Left Up (L1)
worasuchad 6:8ae55e1f7e76 34 Servo Servo2(D5); // Servo Left Down (L2)
worasuchad 6:8ae55e1f7e76 35 Servo Servo3(D8); // Servo Right Up (R1)
worasuchad 6:8ae55e1f7e76 36 Servo Servo4(D9); // Servo Right Down (R2)
Khanchana 0:43d21d5145d3 37
worasuchad 8:865535fcf917 38 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 39 /* prototype fun */
worasuchad 8:865535fcf917 40 /*--------------------------------------------------------------------*/
worasuchad 3:5e867483469e 41 void IMU();
worasuchad 2:18835f8732ad 42 void servo();
worasuchad 8:865535fcf917 43 void servoLeft();
worasuchad 8:865535fcf917 44 void servoRight();
worasuchad 5:08334c6a42ca 45 void servoFirstState();
worasuchad 8:865535fcf917 46 void calcStepDown(float);
worasuchad 8:865535fcf917 47 void calcStepUp(float);
worasuchad 6:8ae55e1f7e76 48 bool checkIMUFirst(float , float, float );
worasuchad 5:08334c6a42ca 49
worasuchad 8:865535fcf917 50 /* debug func */
worasuchad 5:08334c6a42ca 51 void printStateGait();
worasuchad 2:18835f8732ad 52
worasuchad 8:865535fcf917 53 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 54 /* Global variable */
worasuchad 8:865535fcf917 55 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 56 // home variable
worasuchad 5:08334c6a42ca 57 int initCheck = 0;
worasuchad 5:08334c6a42ca 58 float waittime = 0.001 ;
worasuchad 12:a9d894e37936 59 int round = 25;
Khanchana 0:43d21d5145d3 60
worasuchad 11:8548536c3f11 61 // pm variable
worasuchad 11:8548536c3f11 62 int iterPM = 0;
worasuchad 11:8548536c3f11 63 float sumOfPower = 0.0, Energy = 0.0;
worasuchad 11:8548536c3f11 64
worasuchad 8:865535fcf917 65 // interface wt hormone variable
worasuchad 12:a9d894e37936 66 /*
worasuchad 12:a9d894e37936 67 // config flat //
worasuchad 8:865535fcf917 68 float upDeg = 45.00;
worasuchad 8:865535fcf917 69 float downDeg = 95.00;
worasuchad 12:a9d894e37936 70 */
worasuchad 3:5e867483469e 71
worasuchad 12:a9d894e37936 72 // config small //
worasuchad 12:a9d894e37936 73 float upDeg = 60.00;
worasuchad 12:a9d894e37936 74 float downDeg = 100.00;
worasuchad 12:a9d894e37936 75
worasuchad 12:a9d894e37936 76 /*
worasuchad 12:a9d894e37936 77 // config big //
worasuchad 12:a9d894e37936 78 float upDeg = 75.00;
worasuchad 12:a9d894e37936 79 float downDeg = 90.00;
worasuchad 12:a9d894e37936 80 */
worasuchad 6:8ae55e1f7e76 81 // servo motor variable
Khanchana 0:43d21d5145d3 82 float pos_down_start = 1400.00;
Khanchana 0:43d21d5145d3 83 float pos_up_start = 1000.00;
worasuchad 5:08334c6a42ca 84 float stepmin = 1.5;
Khanchana 0:43d21d5145d3 85
worasuchad 5:08334c6a42ca 86 // servo left side
Khanchana 0:43d21d5145d3 87 float pos_down_left = 1400.00;
Khanchana 0:43d21d5145d3 88 float pos_up_left = 1000.00;
Khanchana 0:43d21d5145d3 89 float pos_down_end_left;
Khanchana 0:43d21d5145d3 90 float pos_up_end_left;
Khanchana 0:43d21d5145d3 91 float step_down_left;
Khanchana 0:43d21d5145d3 92 float step_up_left;
worasuchad 6:8ae55e1f7e76 93 int state_count_left = 0;
worasuchad 6:8ae55e1f7e76 94 int round_count_left = 0;
worasuchad 6:8ae55e1f7e76 95
worasuchad 5:08334c6a42ca 96 // servo right side
Khanchana 0:43d21d5145d3 97 float pos_down_right = 1400.00;
Khanchana 0:43d21d5145d3 98 float pos_up_right = 1000.00;
Khanchana 0:43d21d5145d3 99 float pos_down_end_right;
Khanchana 0:43d21d5145d3 100 float pos_up_end_right;
Khanchana 0:43d21d5145d3 101 float step_up_right;
Khanchana 0:43d21d5145d3 102 float step_down_right;
worasuchad 6:8ae55e1f7e76 103 int state_count_right = 0;
worasuchad 6:8ae55e1f7e76 104 int round_count_right = 0;
Khanchana 0:43d21d5145d3 105
worasuchad 11:8548536c3f11 106 // Timer variable
worasuchad 6:8ae55e1f7e76 107 uint32_t getIMUTimer;
worasuchad 8:865535fcf917 108 uint32_t walkingTimer;
worasuchad 6:8ae55e1f7e76 109
worasuchad 8:865535fcf917 110 /* debug variable */
worasuchad 5:08334c6a42ca 111 // print state gait
worasuchad 5:08334c6a42ca 112 int stateGaitOne = 0, stateGaitTwo = 0, stateGaitThree = 0, stateGaitFour = 0;
worasuchad 6:8ae55e1f7e76 113
worasuchad 8:865535fcf917 114 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 115 /* main */
worasuchad 8:865535fcf917 116 /*--------------------------------------------------------------------*/
worasuchad 2:18835f8732ad 117 int main()
worasuchad 2:18835f8732ad 118 {
worasuchad 6:8ae55e1f7e76 119 pc.baud(115200);
worasuchad 8:865535fcf917 120 attitude_setup(); // IMU setup
worasuchad 8:865535fcf917 121 pc.printf(" Please press! '1' to start..\n\r");
worasuchad 2:18835f8732ad 122 if (pc.getc() == '1')
worasuchad 3:5e867483469e 123 {
worasuchad 8:865535fcf917 124 timer1.start(); // start timer counting
worasuchad 6:8ae55e1f7e76 125 thread1.start(IMU); // IMU thread start
worasuchad 11:8548536c3f11 126 thread2.start(servo); // servo thread start
worasuchad 6:8ae55e1f7e76 127 }
worasuchad 3:5e867483469e 128 }
worasuchad 3:5e867483469e 129
worasuchad 8:865535fcf917 130 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 131 /* IMU */
worasuchad 8:865535fcf917 132 /*--------------------------------------------------------------------*/
worasuchad 3:5e867483469e 133 void IMU()
worasuchad 3:5e867483469e 134 {
worasuchad 11:8548536c3f11 135
worasuchad 8:865535fcf917 136 powerMon pmR2(-1);
worasuchad 8:865535fcf917 137 powerMon pmR1(2000);
worasuchad 8:865535fcf917 138 powerMon pmL2(560);
worasuchad 8:865535fcf917 139 powerMon pmL1(3600);
worasuchad 8:865535fcf917 140
worasuchad 8:865535fcf917 141 calculator calc;
worasuchad 8:865535fcf917 142 hormone hg;
worasuchad 8:865535fcf917 143
worasuchad 8:865535fcf917 144 int iterIMU = 0, state_count_left_old = 0, state_count_right_old = 0;
worasuchad 12:a9d894e37936 145 float readTime = 1;
worasuchad 6:8ae55e1f7e76 146 bool IMUWasStable = false;
worasuchad 12:a9d894e37936 147 float *ArrayOfRoll = new float[10];
worasuchad 12:a9d894e37936 148 float *ArrayOfPitch = new float[10];
worasuchad 12:a9d894e37936 149 float *ArrayOfYaw = new float[10];
worasuchad 5:08334c6a42ca 150 float SDOfRoll, SDOfPitch, SDOfYaw, FirstOfRoll, FirstOfPitch, FirstOfYaw;
worasuchad 8:865535fcf917 151 float *vAx, *vAy, *vAz;
worasuchad 8:865535fcf917 152 float *pRoll, *pPitch;
worasuchad 8:865535fcf917 153
worasuchad 8:865535fcf917 154 /* param of State 2 */
worasuchad 8:865535fcf917 155 int iterAG2 = 0;
worasuchad 8:865535fcf917 156 float sdVectorAG2 = 0.0;
worasuchad 8:865535fcf917 157
worasuchad 8:865535fcf917 158 /* param of State 3 */
worasuchad 8:865535fcf917 159 int iterAG3 = 0;
worasuchad 8:865535fcf917 160 float sdVectorAG3 = 0.0;
worasuchad 8:865535fcf917 161
worasuchad 8:865535fcf917 162 /* param of State 4 */
worasuchad 8:865535fcf917 163 int iterG4 = 0;
worasuchad 8:865535fcf917 164 float meanG4 = 0.0;
worasuchad 6:8ae55e1f7e76 165
worasuchad 8:865535fcf917 166 /* memory allocate for G2 and G3 */
worasuchad 12:a9d894e37936 167 vAx = (float *)malloc(60*sizeof(float));
worasuchad 12:a9d894e37936 168 vAy = (float *)malloc(60*sizeof(float));
worasuchad 12:a9d894e37936 169 vAz = (float *)malloc(60*sizeof(float));
worasuchad 8:865535fcf917 170 /* check malloc is not return NULL */
worasuchad 8:865535fcf917 171 if( vAx == NULL or vAy == NULL or vAz == NULL )
worasuchad 8:865535fcf917 172 {
worasuchad 8:865535fcf917 173 pc.printf("Error: memory could not be allocated in vectorA!\n\r");
worasuchad 8:865535fcf917 174 }
worasuchad 8:865535fcf917 175
worasuchad 8:865535fcf917 176 /* memory allocate for G4 */
worasuchad 12:a9d894e37936 177 pRoll = (float *)malloc(80*sizeof(float));
worasuchad 12:a9d894e37936 178 pPitch = (float *)malloc(80*sizeof(float));
worasuchad 8:865535fcf917 179 /* check malloc is not return NULL */
worasuchad 8:865535fcf917 180 if( pRoll == NULL or pPitch == NULL )
worasuchad 8:865535fcf917 181 {
worasuchad 8:865535fcf917 182 pc.printf("Error: memory could not be allocated in pointG4!\n\r");
worasuchad 8:865535fcf917 183 }
worasuchad 8:865535fcf917 184
worasuchad 6:8ae55e1f7e76 185 getIMUTimer = timer1.read_ms();
worasuchad 3:5e867483469e 186 while(1)
worasuchad 3:5e867483469e 187 {
worasuchad 12:a9d894e37936 188 if ((timer1.read_ms() - getIMUTimer) >= readTime) // read time in 1 ms
worasuchad 6:8ae55e1f7e76 189 {
worasuchad 3:5e867483469e 190 attitude_get();
worasuchad 8:865535fcf917 191 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 192 /* Signal Pre-Process every 10 ms */
worasuchad 8:865535fcf917 193 /*--------------------------------------------------------------------*/
worasuchad 6:8ae55e1f7e76 194 if(iterIMU < 10)
worasuchad 3:5e867483469e 195 {
worasuchad 8:865535fcf917 196 if(!IMUWasStable)
worasuchad 8:865535fcf917 197 {
worasuchad 8:865535fcf917 198 ArrayOfRoll[iterIMU] = roll;
worasuchad 8:865535fcf917 199 ArrayOfPitch[iterIMU] = pitch;
worasuchad 8:865535fcf917 200 ArrayOfYaw[iterIMU] = yaw;
worasuchad 8:865535fcf917 201 //pc.printf("%i\t %.3f\t %.3f\t %.3f\n\r",i, roll, pitch, yaw);
worasuchad 8:865535fcf917 202 }
worasuchad 6:8ae55e1f7e76 203 iterIMU++;
worasuchad 3:5e867483469e 204 }
worasuchad 6:8ae55e1f7e76 205 else // every 10 ms
worasuchad 3:5e867483469e 206 {
worasuchad 8:865535fcf917 207 /* print state of gait */
worasuchad 8:865535fcf917 208 //printStateGait();
worasuchad 8:865535fcf917 209
worasuchad 8:865535fcf917 210 /* roll pitch yaw */
worasuchad 8:865535fcf917 211 //pc.printf("%.3f\t\t %.3f\t\t %.3f\n\r", roll, pitch, yaw );
worasuchad 8:865535fcf917 212 //pc.printf("%.3f\t", roll);
worasuchad 8:865535fcf917 213 //pc.printf("%.3f\t", pitch);
worasuchad 8:865535fcf917 214 //pc.printf("%.3f\t\t", yaw);
worasuchad 8:865535fcf917 215
worasuchad 8:865535fcf917 216 /* the accleration value */
worasuchad 8:865535fcf917 217 //pc.printf("%.3f\t", gx*PI/180.0f);
worasuchad 8:865535fcf917 218 //pc.printf("%.3f\t", gy*PI/180.0f);
worasuchad 8:865535fcf917 219 //pc.printf("%.3f\t\t", gz*PI/180.0f);
worasuchad 5:08334c6a42ca 220
worasuchad 8:865535fcf917 221 /* the gyro value */
worasuchad 11:8548536c3f11 222 //pc.printf("%.3f\t", ax * 9.81f ); // convert g to m/s^2
worasuchad 11:8548536c3f11 223 //pc.printf("%.3f\t", ay * 9.81f); // convert g to m/s^2
worasuchad 8:865535fcf917 224 //pc.printf("%.3f\t\t", ( az - 1 ) * 9.81f); // m/s^2 and 1g for eliminating earth gravity
worasuchad 6:8ae55e1f7e76 225
worasuchad 8:865535fcf917 226 /* power monitoring */
worasuchad 8:865535fcf917 227 //pc.printf("%.3f\t", pmL1.Power());
worasuchad 8:865535fcf917 228 //pc.printf("%.3f\t", pmR1.Power());
worasuchad 8:865535fcf917 229 //pc.printf("%.3f\t", pmL2.Power());
worasuchad 8:865535fcf917 230 //pc.printf("%.3f\t\t", pmR2.Power());
worasuchad 8:865535fcf917 231 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 232 /* IMU check whetherbe stable or not before servo begin */
worasuchad 8:865535fcf917 233 /*--------------------------------------------------------------------*/
worasuchad 6:8ae55e1f7e76 234 if(!IMUWasStable)
worasuchad 6:8ae55e1f7e76 235 {
worasuchad 6:8ae55e1f7e76 236 //////////// roll //////////////
worasuchad 8:865535fcf917 237 SDOfRoll = calc.calculateSD(ArrayOfRoll, 10);
worasuchad 8:865535fcf917 238 //SDOfRoll = calculateSD(ArrRoll, 10);
worasuchad 8:865535fcf917 239 //pc.printf("%.3f\t", SDOfRoll);
worasuchad 6:8ae55e1f7e76 240 //pc.printf("%.3f\t\t", ArrayOfRoll[9]);
worasuchad 6:8ae55e1f7e76 241
worasuchad 6:8ae55e1f7e76 242 //////////// pitch ///////////////
worasuchad 8:865535fcf917 243 SDOfPitch = calc.calculateSD(ArrayOfPitch, 10);
worasuchad 8:865535fcf917 244 //SDOfPitch = calculateSD(ArrPitch, 10 );
worasuchad 8:865535fcf917 245 //pc.printf("%.3f\t", SDOfPitch);
worasuchad 6:8ae55e1f7e76 246 //pc.printf("%.3f\t\t", ArrayOfPitch[9]);
worasuchad 6:8ae55e1f7e76 247
worasuchad 6:8ae55e1f7e76 248 //////////// yaw ///////////////
worasuchad 8:865535fcf917 249 SDOfYaw = calc.calculateSD(ArrayOfYaw, 10);
worasuchad 8:865535fcf917 250 //SDOfYaw = calculateSD(ArrYaw, 10 );
worasuchad 6:8ae55e1f7e76 251 //pc.printf("%.3f\n\r", SDOfYaw);
worasuchad 6:8ae55e1f7e76 252 //pc.printf("%.3f\t\t", ArrayOfYaw[9]);
worasuchad 6:8ae55e1f7e76 253 }
worasuchad 11:8548536c3f11 254 else
worasuchad 11:8548536c3f11 255 {
worasuchad 11:8548536c3f11 256 iterPM++;
worasuchad 11:8548536c3f11 257 /*--------------------------------------------------------------------*/
worasuchad 11:8548536c3f11 258 /* power monitoring in J */
worasuchad 11:8548536c3f11 259 /* PM ship can update fastest in 125 ms */
worasuchad 11:8548536c3f11 260 /*--------------------------------------------------------------------*/
worasuchad 11:8548536c3f11 261 //pc.printf("%d\t", iterPM);
worasuchad 11:8548536c3f11 262 //pc.printf("%.3f\t", pmL1.Power());
worasuchad 11:8548536c3f11 263 //pc.printf("%.3f\t", pmR1.Power());
worasuchad 11:8548536c3f11 264 //pc.printf("%.3f\t", pmL2.Power());
worasuchad 11:8548536c3f11 265 //pc.printf("%.3f\n\r", pmR2.Power());
worasuchad 11:8548536c3f11 266 sumOfPower += pmL1.Power();
worasuchad 11:8548536c3f11 267 sumOfPower += pmR1.Power();
worasuchad 11:8548536c3f11 268 sumOfPower += pmL2.Power();
worasuchad 11:8548536c3f11 269 sumOfPower += pmR2.Power();
worasuchad 11:8548536c3f11 270 //sumOfPower = pmL1.Power() + pmR1.Power() + pmL2.Power() + pmR2.Power();
worasuchad 11:8548536c3f11 271 }
worasuchad 5:08334c6a42ca 272
worasuchad 6:8ae55e1f7e76 273 if(checkIMUFirst(SDOfRoll, SDOfPitch, SDOfYaw)) // only one time for comming
worasuchad 6:8ae55e1f7e76 274 {
worasuchad 8:865535fcf917 275 FirstOfRoll = calc.calculateMean(ArrayOfRoll, 10);
worasuchad 8:865535fcf917 276 FirstOfPitch = calc.calculateMean(ArrayOfPitch, 10);
worasuchad 8:865535fcf917 277 FirstOfYaw = calc.calculateMean(ArrayOfYaw, 10);
worasuchad 6:8ae55e1f7e76 278 pc.printf("FirstOfRoll: %.3f, FirstOfPitch: %.3f, FirstOfYaw: %.3f\n\r", FirstOfRoll, FirstOfPitch, FirstOfYaw);
worasuchad 6:8ae55e1f7e76 279
worasuchad 6:8ae55e1f7e76 280 state_count_left = 1;
worasuchad 6:8ae55e1f7e76 281 round_count_left = 1;
worasuchad 6:8ae55e1f7e76 282 state_count_right = 1;
worasuchad 6:8ae55e1f7e76 283 round_count_right = 1;
worasuchad 8:865535fcf917 284 walkingTimer = timer1.read_ms();
worasuchad 6:8ae55e1f7e76 285 IMUWasStable = true;
worasuchad 6:8ae55e1f7e76 286 pc.printf("\n\r:::::::::: IMU was stable ::::::::::\n\r");
worasuchad 12:a9d894e37936 287
worasuchad 12:a9d894e37936 288 delete[] ArrayOfRoll;
worasuchad 12:a9d894e37936 289 delete[] ArrayOfPitch;
worasuchad 12:a9d894e37936 290 delete[] ArrayOfYaw;
worasuchad 12:a9d894e37936 291
worasuchad 12:a9d894e37936 292 readTime = 0.1;
worasuchad 8:865535fcf917 293 }
worasuchad 8:865535fcf917 294
worasuchad 8:865535fcf917 295 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 296 /* || A || in State 2 */
worasuchad 8:865535fcf917 297 /* || A || = sqrt( Ax^2) + Ay^2 + Az^2 ) */
worasuchad 8:865535fcf917 298 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 299
worasuchad 8:865535fcf917 300 if(state_count_left == 2 and state_count_right == 2)
worasuchad 8:865535fcf917 301 {
worasuchad 8:865535fcf917 302 vAx[iterAG2] = ax*9.81f;
worasuchad 8:865535fcf917 303 vAy[iterAG2] = ay*9.81f;
worasuchad 8:865535fcf917 304 vAz[iterAG2] = ( az - 1 ) * 9.81f;
worasuchad 8:865535fcf917 305 iterAG2++;
worasuchad 8:865535fcf917 306 state_count_left_old = state_count_left;
worasuchad 8:865535fcf917 307 state_count_right_old = state_count_right;
worasuchad 8:865535fcf917 308 }
worasuchad 8:865535fcf917 309 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 310 {
worasuchad 8:865535fcf917 311 // calculate SD of size vector A in G2 //
worasuchad 8:865535fcf917 312 sdVectorAG2 = calc.calcSDVectorA(vAx, vAy, vAz, iterAG2);
worasuchad 12:a9d894e37936 313 pc.printf("%.3f\t", sdVectorAG2);
worasuchad 8:865535fcf917 314
worasuchad 8:865535fcf917 315 // hormone concentration //
worasuchad 8:865535fcf917 316 //hg.hormoneCon(sdVectorAG2);
worasuchad 8:865535fcf917 317 //pc.printf("HG2 %.3f\n\r", hg.Cg);
worasuchad 8:865535fcf917 318
worasuchad 8:865535fcf917 319 iterAG2 = 0;
worasuchad 8:865535fcf917 320 state_count_left_old = 0;
worasuchad 8:865535fcf917 321 state_count_right_old = 0;
worasuchad 8:865535fcf917 322 //free(vAz);free(vAy);free(vAz);
worasuchad 8:865535fcf917 323 }
worasuchad 8:865535fcf917 324
worasuchad 8:865535fcf917 325 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 326 /* || A || in State 3 */
worasuchad 8:865535fcf917 327 /* || A || = sqrt( Ax^2) + Ay^2 + Az^2 ) */
worasuchad 8:865535fcf917 328 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 329 else if(state_count_left == 3 and state_count_right == 3)
worasuchad 8:865535fcf917 330 {
worasuchad 8:865535fcf917 331 vAx[iterAG3] = ax*9.81f;
worasuchad 8:865535fcf917 332 vAy[iterAG3] = ay*9.81f;
worasuchad 8:865535fcf917 333 vAz[iterAG3] = ( az - 1 ) * 9.81f;
worasuchad 8:865535fcf917 334 iterAG3++;
worasuchad 8:865535fcf917 335 state_count_left_old = state_count_left;
worasuchad 8:865535fcf917 336 state_count_right_old = state_count_right;
worasuchad 8:865535fcf917 337 }
worasuchad 8:865535fcf917 338 else if(state_count_left_old == 3 and state_count_right_old == 3 and state_count_left == 4 and state_count_right == 4)
worasuchad 8:865535fcf917 339 {
worasuchad 8:865535fcf917 340 // calculate SD of size vector A in G3 //
worasuchad 8:865535fcf917 341 sdVectorAG3 = calc.calcSDVectorA(vAx, vAy, vAz, iterAG3);
worasuchad 12:a9d894e37936 342 pc.printf("%.3f\t", sdVectorAG3);
worasuchad 8:865535fcf917 343
worasuchad 8:865535fcf917 344 // hormone concentration //
worasuchad 8:865535fcf917 345 //hg.hormoneCon(sdVectorAG3);
worasuchad 8:865535fcf917 346 //pc.printf("HG3 %.3f\n\r", hg.Cg);
worasuchad 8:865535fcf917 347
worasuchad 8:865535fcf917 348 iterAG3 = 0;
worasuchad 8:865535fcf917 349 state_count_left_old = 0;
worasuchad 8:865535fcf917 350 state_count_right_old = 0;
worasuchad 8:865535fcf917 351 //free(vAz);free(vAy);free(vAz);
worasuchad 8:865535fcf917 352 }
worasuchad 8:865535fcf917 353
worasuchad 8:865535fcf917 354 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 355 /* (rall,pitch) in State 4 */
worasuchad 8:865535fcf917 356 /* distance form origin (0,0) */
worasuchad 8:865535fcf917 357 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 358
worasuchad 8:865535fcf917 359 else if(state_count_left == 4 and state_count_right == 4 )
worasuchad 8:865535fcf917 360 {
worasuchad 8:865535fcf917 361 pRoll[iterG4] = roll;
worasuchad 8:865535fcf917 362 pPitch[iterG4] = pitch;
worasuchad 12:a9d894e37936 363 //pc.printf("\n %.3f\t", pRoll[iterG4] );
worasuchad 12:a9d894e37936 364 //pc.printf("%.3f\n\r", pPitch[iterG4]);
worasuchad 8:865535fcf917 365 iterG4++;
worasuchad 6:8ae55e1f7e76 366 state_count_left_old = state_count_left;
worasuchad 6:8ae55e1f7e76 367 state_count_right_old = state_count_right;
worasuchad 6:8ae55e1f7e76 368 }
worasuchad 8:865535fcf917 369 else if(state_count_left_old == 4 and state_count_right_old == 4 and state_count_left == 1 and state_count_right == 1)
worasuchad 6:8ae55e1f7e76 370 {
worasuchad 8:865535fcf917 371 // calculate SD of size vector A in G3 //
worasuchad 12:a9d894e37936 372 meanG4 = calc.calcG4(pRoll, pPitch, iterG4);
worasuchad 12:a9d894e37936 373 pc.printf("%.3f\t", meanG4);
worasuchad 8:865535fcf917 374
worasuchad 8:865535fcf917 375 // hormone concentration //
worasuchad 12:a9d894e37936 376 upDeg = hg.upHG(sdVectorAG2, meanG4); // normalize by 2 for SB
worasuchad 8:865535fcf917 377 downDeg = hg.downHG(sdVectorAG2, sdVectorAG3);
worasuchad 12:a9d894e37936 378
worasuchad 12:a9d894e37936 379 pc.printf("%d\t", timer1.read_ms() - walkingTimer);
worasuchad 12:a9d894e37936 380 pc.printf("%.3f\t", sumOfPower);
worasuchad 8:865535fcf917 381 pc.printf("%.3f\t", hg.cgDown);
worasuchad 8:865535fcf917 382 pc.printf("%.3f\t", hg.cgUp);
worasuchad 8:865535fcf917 383 pc.printf("%.3f\t", hg.hormoneRecDown(downDeg));
worasuchad 8:865535fcf917 384 pc.printf("%.3f\n\r", hg.hormoneRecUp(upDeg));
worasuchad 8:865535fcf917 385
worasuchad 8:865535fcf917 386 iterG4 = 0;
worasuchad 6:8ae55e1f7e76 387 state_count_left_old = 0;
worasuchad 6:8ae55e1f7e76 388 state_count_right_old = 0;
worasuchad 8:865535fcf917 389 //free(pRoll);free(pPitch);
worasuchad 6:8ae55e1f7e76 390 }
worasuchad 6:8ae55e1f7e76 391
worasuchad 8:865535fcf917 392 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 393 /* FIN for walking */
worasuchad 8:865535fcf917 394 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 395
worasuchad 8:865535fcf917 396 if (state_count_left == 5 and round_count_left == round and state_count_right == 5 and round_count_right == round)
worasuchad 5:08334c6a42ca 397 {
worasuchad 11:8548536c3f11 398 Energy = sumOfPower * ( (timer1.read_ms() - walkingTimer) / iterPM );
worasuchad 11:8548536c3f11 399
worasuchad 12:a9d894e37936 400 pc.printf("*\t");
worasuchad 12:a9d894e37936 401 pc.printf("%d\t", timer1.read_ms() - walkingTimer);
worasuchad 12:a9d894e37936 402 pc.printf("%.3f\t", sumOfPower);
worasuchad 11:8548536c3f11 403 pc.printf("%.3f\t", hg.cgDown);
worasuchad 11:8548536c3f11 404 pc.printf("%.3f\t", hg.cgUp);
worasuchad 11:8548536c3f11 405 pc.printf("%.3f\t", hg.hormoneRecDown(downDeg));
worasuchad 11:8548536c3f11 406 pc.printf("%.3f\n\r", hg.hormoneRecUp(upDeg));
worasuchad 11:8548536c3f11 407
worasuchad 8:865535fcf917 408 pc.printf("TIME %d \n\r", timer1.read_ms() - walkingTimer);
worasuchad 11:8548536c3f11 409 pc.printf("E %.3f \n\r", Energy / 1000.00f);
worasuchad 11:8548536c3f11 410 pc.printf("P %.3f \n\r", sumOfPower);
worasuchad 11:8548536c3f11 411 pc.printf("ITER PM %d \n\r", iterPM);
worasuchad 11:8548536c3f11 412 pc.printf("FIN IMU!\n\r");
worasuchad 8:865535fcf917 413 //thread1.terminate();
worasuchad 8:865535fcf917 414 free(vAz);free(vAy);free(vAz);
worasuchad 8:865535fcf917 415 free(pRoll);free(pPitch);
worasuchad 8:865535fcf917 416 break;
worasuchad 5:08334c6a42ca 417 }
worasuchad 8:865535fcf917 418
worasuchad 3:5e867483469e 419 // reset iteration
worasuchad 6:8ae55e1f7e76 420 iterIMU = 0;
worasuchad 3:5e867483469e 421 }
worasuchad 6:8ae55e1f7e76 422 getIMUTimer = timer1.read_ms();
worasuchad 3:5e867483469e 423 }
worasuchad 2:18835f8732ad 424 }
worasuchad 3:5e867483469e 425 }
worasuchad 3:5e867483469e 426
worasuchad 8:865535fcf917 427 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 428 /* servo */
worasuchad 8:865535fcf917 429 /*--------------------------------------------------------------------*/
worasuchad 3:5e867483469e 430 void servo()
worasuchad 8:865535fcf917 431 {
worasuchad 8:865535fcf917 432 hormone hr;
worasuchad 8:865535fcf917 433
worasuchad 6:8ae55e1f7e76 434 Servo1.Enable(1000,20000);
worasuchad 3:5e867483469e 435 Servo2.Enable(1000,20000);
worasuchad 3:5e867483469e 436 Servo3.Enable(1000,20000);
worasuchad 6:8ae55e1f7e76 437 Servo4.Enable(1000,20000);
worasuchad 3:5e867483469e 438
worasuchad 6:8ae55e1f7e76 439 Servo1.SetPosition(pos_down_left);
worasuchad 6:8ae55e1f7e76 440 Servo2.SetPosition(pos_up_left);
worasuchad 6:8ae55e1f7e76 441 Servo3.SetPosition(pos_down_right);
worasuchad 6:8ae55e1f7e76 442 Servo4.SetPosition(pos_up_right);
worasuchad 5:08334c6a42ca 443
worasuchad 3:5e867483469e 444 while(1)
worasuchad 3:5e867483469e 445 {
worasuchad 8:865535fcf917 446 calcStepUp( hr.hormoneRecUp(upDeg) ); // return "step_up_right" and "step_up_left"
worasuchad 8:865535fcf917 447 calcStepDown( hr.hormoneRecDown(downDeg) ); // return "step_down_right" and "step_down_left"
worasuchad 8:865535fcf917 448 servoLeft(); // control left lag
worasuchad 8:865535fcf917 449 servoRight(); // control right leg
worasuchad 3:5e867483469e 450
worasuchad 6:8ae55e1f7e76 451 // FIN for walking
worasuchad 6:8ae55e1f7e76 452 if (state_count_left == 5 and round_count_left == round and state_count_right == 5 and round_count_right == round)
worasuchad 3:5e867483469e 453 {
worasuchad 11:8548536c3f11 454 //pc.printf("FIN SERVO! \n\r");
worasuchad 8:865535fcf917 455 //thread2.terminate();
worasuchad 6:8ae55e1f7e76 456 break;
worasuchad 3:5e867483469e 457 }
worasuchad 3:5e867483469e 458 }
worasuchad 3:5e867483469e 459 }
Khanchana 0:43d21d5145d3 460
worasuchad 8:865535fcf917 461 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 462 /* calculate step of servo down */
worasuchad 8:865535fcf917 463 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 464 void calcStepDown(float hormDown)
worasuchad 2:18835f8732ad 465 {
worasuchad 8:865535fcf917 466 pos_down_end_left = (1000.00 + ((700.00/90.00)*( hormDown ))); // get degree for hormone receiver about downDegree ~ 90*,
worasuchad 8:865535fcf917 467 pos_down_end_right = (1060.00 + ((700.00/90.00)*( hormDown ))); // so both pos_down_end_left and pos_down_end_right are around 1700
worasuchad 2:18835f8732ad 468 if (pos_down_end_right > pos_down_end_left)
worasuchad 2:18835f8732ad 469 {
worasuchad 2:18835f8732ad 470 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 471 step_down_left = stepmin;
worasuchad 2:18835f8732ad 472 }
worasuchad 2:18835f8732ad 473 else if (pos_down_end_right < pos_down_end_left)
worasuchad 2:18835f8732ad 474 {
Khanchana 0:43d21d5145d3 475 step_down_right = stepmin;
Khanchana 0:43d21d5145d3 476 step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
worasuchad 2:18835f8732ad 477 }
worasuchad 2:18835f8732ad 478 else // pos_down_end_right == pos_down_end_left
worasuchad 2:18835f8732ad 479 {
Khanchana 0:43d21d5145d3 480 step_down_right = stepmin;
Khanchana 0:43d21d5145d3 481 step_down_left = stepmin;
Khanchana 0:43d21d5145d3 482 }
Khanchana 0:43d21d5145d3 483 }
worasuchad 2:18835f8732ad 484
worasuchad 8:865535fcf917 485 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 486 /* calculate step of servo up */
worasuchad 8:865535fcf917 487 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 488 void calcStepUp(float hormUp)
worasuchad 8:865535fcf917 489 {
worasuchad 8:865535fcf917 490 pos_up_end_left = 1000.00 + ((700.00/90.00)*( hormUp )); // get degree for hormone receiver about upDegree ~ 45*,
worasuchad 8:865535fcf917 491 pos_up_end_right = 1000.00 + ((700.00/90.00)*( hormUp )); // so both pos_up_end_left and pos_up_end_right are around 1350
worasuchad 2:18835f8732ad 492 if (pos_up_end_right > pos_up_end_left)
worasuchad 2:18835f8732ad 493 {
worasuchad 2:18835f8732ad 494 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 495 step_up_left = stepmin;
worasuchad 2:18835f8732ad 496 }
worasuchad 2:18835f8732ad 497 else if (pos_up_end_right < pos_up_end_left)
worasuchad 2:18835f8732ad 498 {
Khanchana 0:43d21d5145d3 499 step_up_right = stepmin;
Khanchana 0:43d21d5145d3 500 step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
worasuchad 2:18835f8732ad 501 }
worasuchad 2:18835f8732ad 502 else // step_up_right == step_up_left
worasuchad 2:18835f8732ad 503 {
Khanchana 0:43d21d5145d3 504 step_up_right = stepmin;
Khanchana 0:43d21d5145d3 505 step_up_left = stepmin;
Khanchana 0:43d21d5145d3 506 }
worasuchad 5:08334c6a42ca 507 }
worasuchad 5:08334c6a42ca 508
worasuchad 8:865535fcf917 509 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 510 /* servo in left side */
worasuchad 8:865535fcf917 511 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 512 void servoLeft()
worasuchad 2:18835f8732ad 513 {
worasuchad 3:5e867483469e 514 if(state_count_left == 1)
worasuchad 2:18835f8732ad 515 {
worasuchad 2:18835f8732ad 516 Servo1.SetPosition(pos_down_left); // pos_down_left = 1400.00
worasuchad 2:18835f8732ad 517 wait(waittime); // 0.001 ms
worasuchad 3:5e867483469e 518 pos_down_left += step_down_left;
worasuchad 2:18835f8732ad 519 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 520 {
Khanchana 0:43d21d5145d3 521 state_count_left = 2;
Khanchana 0:43d21d5145d3 522 }
worasuchad 2:18835f8732ad 523 }
worasuchad 2:18835f8732ad 524 else if(state_count_left == 2)
worasuchad 2:18835f8732ad 525 {
worasuchad 2:18835f8732ad 526 Servo2.SetPosition(pos_up_left); // pos_up_left = 1000.00
Khanchana 0:43d21d5145d3 527 wait(waittime);
worasuchad 3:5e867483469e 528 pos_up_left += step_up_left;
worasuchad 2:18835f8732ad 529 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 530 {
Khanchana 0:43d21d5145d3 531 state_count_left = 3;
Khanchana 0:43d21d5145d3 532 }
worasuchad 2:18835f8732ad 533 }
worasuchad 2:18835f8732ad 534 else if(state_count_left == 3)
worasuchad 2:18835f8732ad 535 {
Khanchana 0:43d21d5145d3 536 Servo1.SetPosition(pos_down_left);
Khanchana 0:43d21d5145d3 537 wait(waittime);
worasuchad 3:5e867483469e 538 pos_down_left -= step_down_left;
worasuchad 2:18835f8732ad 539 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left)
worasuchad 2:18835f8732ad 540 {
Khanchana 0:43d21d5145d3 541 state_count_left = 4;
Khanchana 0:43d21d5145d3 542 }
worasuchad 2:18835f8732ad 543 }
worasuchad 2:18835f8732ad 544 else if(state_count_left == 4)
worasuchad 2:18835f8732ad 545 {
Khanchana 0:43d21d5145d3 546 Servo2.SetPosition(pos_up_left);
Khanchana 0:43d21d5145d3 547 wait(waittime);
worasuchad 3:5e867483469e 548 pos_up_left -= step_up_left;
worasuchad 2:18835f8732ad 549 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left)
worasuchad 2:18835f8732ad 550 {
worasuchad 6:8ae55e1f7e76 551 state_count_left = 5;
Khanchana 0:43d21d5145d3 552 }
worasuchad 2:18835f8732ad 553 }
worasuchad 6:8ae55e1f7e76 554 else if (state_count_left == 5 and round_count_left < round)
worasuchad 2:18835f8732ad 555 {
worasuchad 3:5e867483469e 556 round_count_left++;
Khanchana 0:43d21d5145d3 557 state_count_left = 1;
Khanchana 0:43d21d5145d3 558 pos_down_left = pos_down_start;
Khanchana 0:43d21d5145d3 559 pos_up_left = pos_up_start;
worasuchad 2:18835f8732ad 560 }
Khanchana 0:43d21d5145d3 561 }
worasuchad 2:18835f8732ad 562
worasuchad 8:865535fcf917 563 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 564 /* servo in right side */
worasuchad 8:865535fcf917 565 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 566 void servoRight()
Khanchana 0:43d21d5145d3 567 {
worasuchad 2:18835f8732ad 568 if(state_count_right == 1)
worasuchad 2:18835f8732ad 569 {
Khanchana 0:43d21d5145d3 570 Servo3.SetPosition(pos_down_right);
Khanchana 0:43d21d5145d3 571 wait(waittime);
Khanchana 0:43d21d5145d3 572 pos_down_right = pos_down_right + step_down_right;
worasuchad 2:18835f8732ad 573 if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start)
worasuchad 2:18835f8732ad 574 {
Khanchana 0:43d21d5145d3 575 state_count_right = 2;
Khanchana 0:43d21d5145d3 576 }
worasuchad 2:18835f8732ad 577 }
worasuchad 2:18835f8732ad 578 else if(state_count_right == 2)
worasuchad 2:18835f8732ad 579 {
Khanchana 0:43d21d5145d3 580 Servo4.SetPosition(pos_up_right);
Khanchana 0:43d21d5145d3 581 wait(waittime);
Khanchana 0:43d21d5145d3 582 pos_up_right = pos_up_right + step_up_right;
worasuchad 2:18835f8732ad 583 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 584 {
Khanchana 0:43d21d5145d3 585 state_count_right = 3;
Khanchana 0:43d21d5145d3 586 }
worasuchad 2:18835f8732ad 587 }
worasuchad 2:18835f8732ad 588 else if(state_count_right == 3)
worasuchad 2:18835f8732ad 589 {
Khanchana 0:43d21d5145d3 590 Servo3.SetPosition(pos_down_right);
Khanchana 0:43d21d5145d3 591 wait(waittime);
Khanchana 0:43d21d5145d3 592 pos_down_right = pos_down_right - step_down_right;
worasuchad 2:18835f8732ad 593 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right)
worasuchad 2:18835f8732ad 594 {
Khanchana 0:43d21d5145d3 595 state_count_right = 4;
Khanchana 0:43d21d5145d3 596 }
worasuchad 2:18835f8732ad 597 }
worasuchad 2:18835f8732ad 598 else if(state_count_right == 4)
worasuchad 2:18835f8732ad 599 {
Khanchana 0:43d21d5145d3 600 Servo4.SetPosition(pos_up_right);
worasuchad 2:18835f8732ad 601 wait(waittime);
Khanchana 0:43d21d5145d3 602 pos_up_right = pos_up_right - step_up_right;
worasuchad 2:18835f8732ad 603 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right)
worasuchad 2:18835f8732ad 604 {
worasuchad 6:8ae55e1f7e76 605 state_count_right = 5;
Khanchana 0:43d21d5145d3 606 }
worasuchad 2:18835f8732ad 607 }
worasuchad 6:8ae55e1f7e76 608 else if (state_count_right == 5 and round_count_right < round)
worasuchad 2:18835f8732ad 609 {
Khanchana 0:43d21d5145d3 610 round_count_right = round_count_right+1;
Khanchana 0:43d21d5145d3 611 state_count_right = 1;
Khanchana 0:43d21d5145d3 612 pos_down_right = pos_down_start;
Khanchana 0:43d21d5145d3 613 pos_up_right = pos_up_start;
Khanchana 0:43d21d5145d3 614 }
Khanchana 0:43d21d5145d3 615 }
Khanchana 0:43d21d5145d3 616
worasuchad 8:865535fcf917 617 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 618 /* check IMU first */
worasuchad 8:865535fcf917 619 /*--------------------------------------------------------------------*/
worasuchad 6:8ae55e1f7e76 620 bool checkIMUFirst(float SDOfRoll, float SDOfPitch, float SDOfYaw)
worasuchad 6:8ae55e1f7e76 621 {
worasuchad 6:8ae55e1f7e76 622 if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 0)
worasuchad 6:8ae55e1f7e76 623 {
worasuchad 6:8ae55e1f7e76 624 initCheck = 1;
worasuchad 6:8ae55e1f7e76 625 return true;
worasuchad 6:8ae55e1f7e76 626 }
worasuchad 6:8ae55e1f7e76 627 return false;
worasuchad 6:8ae55e1f7e76 628 }
worasuchad 6:8ae55e1f7e76 629
worasuchad 8:865535fcf917 630 /*--------------------------------------------------------------------*/
worasuchad 8:865535fcf917 631 /* print state gait for debuging */
worasuchad 8:865535fcf917 632 /*--------------------------------------------------------------------*/
worasuchad 6:8ae55e1f7e76 633 void printStateGait()
worasuchad 3:5e867483469e 634 {
worasuchad 6:8ae55e1f7e76 635 if(state_count_left == 1 and state_count_right == 1 and stateGaitOne == 0)
worasuchad 3:5e867483469e 636 {
worasuchad 8:865535fcf917 637 //pc.printf("\n\r State Gait 1 \n\r");
worasuchad 8:865535fcf917 638 pc.printf("\n\rG1\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \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 8:865535fcf917 643 //pc.printf("\n\r State Gait 2 \n\r");
worasuchad 8:865535fcf917 644 pc.printf("\n\rG2\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \n\r");
worasuchad 6:8ae55e1f7e76 645 stateGaitOne = 0; stateGaitTwo = 1; stateGaitThree = 0; stateGaitFour = 0;
worasuchad 5:08334c6a42ca 646 }
worasuchad 6:8ae55e1f7e76 647 else if(state_count_left == 3 and state_count_right == 3 and stateGaitThree == 0)
worasuchad 6:8ae55e1f7e76 648 {
worasuchad 8:865535fcf917 649 //pc.printf("\n\r State Gait 3 \n\r");
worasuchad 8:865535fcf917 650 pc.printf("\n\rG3\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \n\r");
worasuchad 6:8ae55e1f7e76 651 stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 1; stateGaitFour = 0;
worasuchad 6:8ae55e1f7e76 652 }
worasuchad 6:8ae55e1f7e76 653 else if(state_count_left == 4 and state_count_right == 4 and stateGaitFour == 0)
worasuchad 6:8ae55e1f7e76 654 {
worasuchad 8:865535fcf917 655 //pc.printf("\n\r State Gait 4 \n\r");
worasuchad 8:865535fcf917 656 pc.printf("\n\rG4\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \n\r");
worasuchad 6:8ae55e1f7e76 657 stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 1;
worasuchad 6:8ae55e1f7e76 658 }
worasuchad 8:865535fcf917 659 }
worasuchad 8:865535fcf917 660 /*--------------------------------------------------------------------*/