worasuchad haomachai / Mbed 2 deprecated TurtleBot_with_AHS

Update hormone config

Dependencies:   mbed Servo PM mbed-rtos hormone calculator

main.cpp@14:d084ea982238, 2019-11-10 (annotated)

Committer:
worasuchad
Date:
Sun Nov 10 02:45:05 2019 +0000
Revision:
14:d084ea982238
Parent:
12:a9d894e37936 
add config;

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