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@8:865535fcf917, 2018-08-13 (annotated)

Committer:
worasuchad
Date:
Mon Aug 13 11:39:16 2018 +0000
Revision:
8:865535fcf917
Parent:
7:ca887fdc5388
Child:
11:8548536c3f11
hormone v.1 for optimizing COT

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