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@11:8548536c3f11, 2018-08-19 (annotated)

Committer:
worasuchad
Date:
Sun Aug 19 06:28:40 2018 +0000
Revision:
11:8548536c3f11
Parent:
8:865535fcf917
Child:
12:a9d894e37936
hormone experiment

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