worasuchad haomachai
Update hormone config
Dependencies: mbed Servo PM mbed-rtos hormone calculator
main.cpp
- Committer:
- worasuchad
- Date:
- 2019-11-10
- Revision:
- 14:d084ea982238
- Parent:
- 12:a9d894e37936
File content as of revision 14:d084ea982238:
/***************************< File comment >***************************/
/* project: TurtleBot Project */
/* project description : - */
/*--------------------------------------------------------------------*/
/* version : 0.9 */
/* board : NUCLEO-F411RE */
/* detail : two hg is added */
/*--------------------------------------------------------------------*/
/* create : 19/08/2018 */
/* programmer : Worasuchad Haomachai */
/**********************************************************************/
/*--------------------------------------------------------------------*/
/* Include file */
/*--------------------------------------------------------------------*/
#include <stdlib.h>
#include "mbed.h"
#include "Servo.h"
#include "rtos.h"
#include "attitude.h"
#include "math.h"
#include "calculator.h"
#include "hormone.h"
#include "PowerMon.h"
#define INITCONFIG 3 // 0-default, 1-flat, 2-small, 3-big roughtness
Serial pc(USBTX, USBRX); // Serial Port
Timer timer1;
Thread thread1;
Thread thread2;
Thread thread3;
Servo Servo1(D4); // Servo Left Up (L1)
Servo Servo2(D5); // Servo Left Down (L2)
Servo Servo3(D8); // Servo Right Up (R1)
Servo Servo4(D9); // Servo Right Down (R2)
/*--------------------------------------------------------------------*/
/* prototype fun */
/*--------------------------------------------------------------------*/
void IMU();
void servo();
void servoLeft();
void servoRight();
void servoFirstState();
void calcStepDown(float);
void calcStepUp(float);
bool checkIMUFirst(float , float, float );
/* debug func */
void printStateGait();
/*--------------------------------------------------------------------*/
/* Global variable */
/*--------------------------------------------------------------------*/
// home variable
int initCheck = 0;
float waittime = 0.001 ;
int round = 8;
// pm variable
int iterPM = 0;
float sumOfPower = 0.0, Energy = 0.0;
// interface wt hormone variable
#if(INITCONFIG == 0)
// default config //
float upDeg = 45.00;
float downDeg = 90.00;
#endif
#if(INITCONFIG == 1)
// config flat //
float upDeg = 45.00;
float downDeg = 95.00;
#endif
#if(INITCONFIG == 2)
// config small //
float upDeg = 60.00;
float downDeg = 100.00;
#endif
#if(INITCONFIG == 3)
// config big //
float upDeg = 75.00;
float downDeg = 90.00;
#endif
// servo motor variable
float pos_down_start = 1400.00;
float pos_up_start = 1000.00;
float stepmin = 1.5;
// servo left side
float pos_down_left = 1400.00;
float pos_up_left = 1000.00;
float pos_down_end_left;
float pos_up_end_left;
float step_down_left;
float step_up_left;
int state_count_left = 0;
int round_count_left = 0;
// servo right side
float pos_down_right = 1400.00;
float pos_up_right = 1000.00;
float pos_down_end_right;
float pos_up_end_right;
float step_up_right;
float step_down_right;
int state_count_right = 0;
int round_count_right = 0;
// Timer variable
uint32_t getIMUTimer;
uint32_t walkingTimer;
/* debug variable */
// print state gait
int stateGaitOne = 0, stateGaitTwo = 0, stateGaitThree = 0, stateGaitFour = 0;
/*--------------------------------------------------------------------*/
/* main */
/*--------------------------------------------------------------------*/
int main()
{
pc.baud(115200);
attitude_setup(); // IMU setup
pc.printf(" Please press! '1' to start..\n\r");
if (pc.getc() == '1')
{
timer1.start(); // start timer counting
thread1.start(IMU); // IMU thread start
thread2.start(servo); // servo thread start
}
}
/*--------------------------------------------------------------------*/
/* IMU */
/*--------------------------------------------------------------------*/
void IMU()
{
powerMon pmR2(-1);
powerMon pmR1(2000);
powerMon pmL2(560);
powerMon pmL1(3600);
calculator calc;
hormone hg;
int iterIMU = 0, state_count_left_old = 0, state_count_right_old = 0;
float readTime = 1;
bool IMUWasStable = false;
float *ArrayOfRoll = new float[10];
float *ArrayOfPitch = new float[10];
float *ArrayOfYaw = new float[10];
float SDOfRoll, SDOfPitch, SDOfYaw, FirstOfRoll, FirstOfPitch, FirstOfYaw;
float *vAx, *vAy, *vAz;
float *pRoll, *pPitch;
/* param of State 2 */
int iterAG2 = 0;
float sdVectorAG2 = 0.0;
/* param of State 3 */
int iterAG3 = 0;
float sdVectorAG3 = 0.0;
/* param of State 4 */
int iterG4 = 0;
float meanG4 = 0.0;
/* memory allocate for G2 and G3 */
vAx = (float *)malloc(60*sizeof(float));
vAy = (float *)malloc(60*sizeof(float));
vAz = (float *)malloc(60*sizeof(float));
/* check malloc is not return NULL */
if( vAx == NULL or vAy == NULL or vAz == NULL )
{
pc.printf("Error: memory could not be allocated in vectorA!\n\r");
}
/* memory allocate for G4 */
pRoll = (float *)malloc(80*sizeof(float));
pPitch = (float *)malloc(80*sizeof(float));
/* check malloc is not return NULL */
if( pRoll == NULL or pPitch == NULL )
{
pc.printf("Error: memory could not be allocated in pointG4!\n\r");
}
getIMUTimer = timer1.read_ms();
while(1)
{
if ((timer1.read_ms() - getIMUTimer) >= readTime) // read time in 1 ms
{
attitude_get();
/*--------------------------------------------------------------------*/
/* Signal Pre-Process every 10 ms */
/*--------------------------------------------------------------------*/
if(iterIMU < 10)
{
if(!IMUWasStable)
{
ArrayOfRoll[iterIMU] = roll;
ArrayOfPitch[iterIMU] = pitch;
ArrayOfYaw[iterIMU] = yaw;
//pc.printf("%i\t %.3f\t %.3f\t %.3f\n\r",i, roll, pitch, yaw);
}
iterIMU++;
}
else // every 10 ms
{
/* print state of gait */
//printStateGait();
/* roll pitch yaw */
//pc.printf("%.3f\t\t %.3f\t\t %.3f\n\r", roll, pitch, yaw );
//pc.printf("%.3f\t", roll);
//pc.printf("%.3f\t", pitch);
//pc.printf("%.3f\t\t", yaw);
/* the accleration value */
//pc.printf("%.3f\t", gx*PI/180.0f);
//pc.printf("%.3f\t", gy*PI/180.0f);
//pc.printf("%.3f\t\t", gz*PI/180.0f);
/* the gyro value */
//pc.printf("%.3f\t", ax * 9.81f ); // convert g to m/s^2
//pc.printf("%.3f\t", ay * 9.81f); // convert g to m/s^2
//pc.printf("%.3f\t\t", ( az - 1 ) * 9.81f); // m/s^2 and 1g for eliminating earth gravity
/* power monitoring */
//pc.printf("%.3f\t", pmL1.Power());
//pc.printf("%.3f\t", pmR1.Power());
//pc.printf("%.3f\t", pmL2.Power());
//pc.printf("%.3f\t\t", pmR2.Power());
/*--------------------------------------------------------------------*/
/* IMU check whetherbe stable or not before servo begin */
/*--------------------------------------------------------------------*/
if(!IMUWasStable)
{
//////////// roll //////////////
SDOfRoll = calc.calculateSD(ArrayOfRoll, 10);
//SDOfRoll = calculateSD(ArrRoll, 10);
//pc.printf("%.3f\t", SDOfRoll);
//pc.printf("%.3f\t\t", ArrayOfRoll[9]);
//////////// pitch ///////////////
SDOfPitch = calc.calculateSD(ArrayOfPitch, 10);
//SDOfPitch = calculateSD(ArrPitch, 10 );
//pc.printf("%.3f\t", SDOfPitch);
//pc.printf("%.3f\t\t", ArrayOfPitch[9]);
//////////// yaw ///////////////
SDOfYaw = calc.calculateSD(ArrayOfYaw, 10);
//SDOfYaw = calculateSD(ArrYaw, 10 );
//pc.printf("%.3f\n\r", SDOfYaw);
//pc.printf("%.3f\t\t", ArrayOfYaw[9]);
}
else
{
iterPM++;
/*--------------------------------------------------------------------*/
/* power monitoring in J */
/* PM ship can update fastest in 125 ms */
/*--------------------------------------------------------------------*/
//pc.printf("%d\t", iterPM);
//pc.printf("%.3f\t", pmL1.Power());
//pc.printf("%.3f\t", pmR1.Power());
//pc.printf("%.3f\t", pmL2.Power());
//pc.printf("%.3f\n\r", pmR2.Power());
sumOfPower += pmL1.Power();
sumOfPower += pmR1.Power();
sumOfPower += pmL2.Power();
sumOfPower += pmR2.Power();
//sumOfPower = pmL1.Power() + pmR1.Power() + pmL2.Power() + pmR2.Power();
}
if(checkIMUFirst(SDOfRoll, SDOfPitch, SDOfYaw)) // only one time for comming
{
FirstOfRoll = calc.calculateMean(ArrayOfRoll, 10);
FirstOfPitch = calc.calculateMean(ArrayOfPitch, 10);
FirstOfYaw = calc.calculateMean(ArrayOfYaw, 10);
pc.printf("FirstOfRoll: %.3f, FirstOfPitch: %.3f, FirstOfYaw: %.3f\n\r", FirstOfRoll, FirstOfPitch, FirstOfYaw);
state_count_left = 1;
round_count_left = 1;
state_count_right = 1;
round_count_right = 1;
walkingTimer = timer1.read_ms();
IMUWasStable = true;
pc.printf("\n\r:::::::::: IMU was stable ::::::::::\n\r");
delete[] ArrayOfRoll;
delete[] ArrayOfPitch;
delete[] ArrayOfYaw;
readTime = 0.1;
}
/*--------------------------------------------------------------------*/
/* || A || in State 2 */
/* || A || = sqrt( Ax^2) + Ay^2 + Az^2 ) */
/*--------------------------------------------------------------------*/
if(state_count_left == 2 and state_count_right == 2)
{
vAx[iterAG2] = ax*9.81f;
vAy[iterAG2] = ay*9.81f;
vAz[iterAG2] = ( az - 1 ) * 9.81f;
iterAG2++;
state_count_left_old = state_count_left;
state_count_right_old = state_count_right;
}
else if(state_count_left_old == 2 and state_count_right_old == 2 and state_count_left == 3 and state_count_right == 3)
{
// calculate SD of size vector A in G2 //
sdVectorAG2 = calc.calcSDVectorA(vAx, vAy, vAz, iterAG2);
pc.printf("%.3f\t", sdVectorAG2);
// hormone concentration //
//hg.hormoneCon(sdVectorAG2);
//pc.printf("HG2 %.3f\n\r", hg.Cg);
iterAG2 = 0;
state_count_left_old = 0;
state_count_right_old = 0;
//free(vAz);free(vAy);free(vAz);
}
/*--------------------------------------------------------------------*/
/* || A || in State 3 */
/* || A || = sqrt( Ax^2) + Ay^2 + Az^2 ) */
/*--------------------------------------------------------------------*/
else if(state_count_left == 3 and state_count_right == 3)
{
vAx[iterAG3] = ax*9.81f;
vAy[iterAG3] = ay*9.81f;
vAz[iterAG3] = ( az - 1 ) * 9.81f;
iterAG3++;
state_count_left_old = state_count_left;
state_count_right_old = state_count_right;
}
else if(state_count_left_old == 3 and state_count_right_old == 3 and state_count_left == 4 and state_count_right == 4)
{
// calculate SD of size vector A in G3 //
sdVectorAG3 = calc.calcSDVectorA(vAx, vAy, vAz, iterAG3);
pc.printf("%.3f\t", sdVectorAG3);
// hormone concentration //
//hg.hormoneCon(sdVectorAG3);
//pc.printf("HG3 %.3f\n\r", hg.Cg);
iterAG3 = 0;
state_count_left_old = 0;
state_count_right_old = 0;
//free(vAz);free(vAy);free(vAz);
}
/*--------------------------------------------------------------------*/
/* (rall,pitch) in State 4 */
/* distance form origin (0,0) */
/*--------------------------------------------------------------------*/
else if(state_count_left == 4 and state_count_right == 4 )
{
pRoll[iterG4] = roll;
pPitch[iterG4] = pitch;
//pc.printf("\n %.3f\t", pRoll[iterG4] );
//pc.printf("%.3f\n\r", pPitch[iterG4]);
iterG4++;
state_count_left_old = state_count_left;
state_count_right_old = state_count_right;
}
else if(state_count_left_old == 4 and state_count_right_old == 4 and state_count_left == 1 and state_count_right == 1)
{
// calculate SD of size vector A in G3 //
meanG4 = calc.calcG4(pRoll, pPitch, iterG4);
pc.printf("%.3f\t", meanG4);
// hormone concentration //
upDeg = hg.upHG(sdVectorAG2, meanG4); // normalize by 2 for SB
downDeg = hg.downHG(sdVectorAG2, sdVectorAG3);
pc.printf("%d\t", timer1.read_ms() - walkingTimer);
pc.printf("%.3f\t", sumOfPower);
pc.printf("%.3f\t", hg.cgDown);
pc.printf("%.3f\t", hg.cgUp);
pc.printf("%.3f\t", hg.hormoneRecDown(downDeg));
pc.printf("%.3f\n\r", hg.hormoneRecUp(upDeg));
iterG4 = 0;
state_count_left_old = 0;
state_count_right_old = 0;
//free(pRoll);free(pPitch);
}
/*--------------------------------------------------------------------*/
/* FIN for walking */
/*--------------------------------------------------------------------*/
if (state_count_left == 5 and round_count_left == round and state_count_right == 5 and round_count_right == round)
{
Energy = sumOfPower * ( (timer1.read_ms() - walkingTimer) / iterPM );
pc.printf("*\t");
pc.printf("%d\t", timer1.read_ms() - walkingTimer);
pc.printf("%.3f\t", sumOfPower);
pc.printf("%.3f\t", hg.cgDown);
pc.printf("%.3f\t", hg.cgUp);
pc.printf("%.3f\t", hg.hormoneRecDown(downDeg));
pc.printf("%.3f\n\r", hg.hormoneRecUp(upDeg));
pc.printf("TIME %d \n\r", timer1.read_ms() - walkingTimer);
pc.printf("E %.3f \n\r", Energy / 1000.00f);
pc.printf("P %.3f \n\r", sumOfPower);
pc.printf("ITER PM %d \n\r", iterPM);
pc.printf("FIN IMU!\n\r");
//thread1.terminate();
free(vAz);free(vAy);free(vAz);
free(pRoll);free(pPitch);
break;
}
// reset iteration
iterIMU = 0;
}
getIMUTimer = timer1.read_ms();
}
}
}
/*--------------------------------------------------------------------*/
/* servo */
/*--------------------------------------------------------------------*/
void servo()
{
hormone hr;
Servo1.Enable(1000,20000);
Servo2.Enable(1000,20000);
Servo3.Enable(1000,20000);
Servo4.Enable(1000,20000);
Servo1.SetPosition(pos_down_left);
Servo2.SetPosition(pos_up_left);
Servo3.SetPosition(pos_down_right);
Servo4.SetPosition(pos_up_right);
while(1)
{
calcStepUp( hr.hormoneRecUp(upDeg) ); // return "step_up_right" and "step_up_left"
calcStepDown( hr.hormoneRecDown(downDeg) ); // return "step_down_right" and "step_down_left"
servoLeft(); // control left lag
servoRight(); // control right leg
// FIN for walking
if (state_count_left == 5 and round_count_left == round and state_count_right == 5 and round_count_right == round)
{
//pc.printf("FIN SERVO! \n\r");
//thread2.terminate();
break;
}
}
}
/*--------------------------------------------------------------------*/
/* calculate step of servo down */
/*--------------------------------------------------------------------*/
void calcStepDown(float hormDown)
{
pos_down_end_left = (1000.00 + ((700.00/90.00)*( hormDown ))); // get degree for hormone receiver about downDegree ~ 90*,
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
if (pos_down_end_right > pos_down_end_left)
{
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
step_down_left = stepmin;
}
else if (pos_down_end_right < pos_down_end_left)
{
step_down_right = stepmin;
step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
}
else // pos_down_end_right == pos_down_end_left
{
step_down_right = stepmin;
step_down_left = stepmin;
}
}
/*--------------------------------------------------------------------*/
/* calculate step of servo up */
/*--------------------------------------------------------------------*/
void calcStepUp(float hormUp)
{
pos_up_end_left = 1000.00 + ((700.00/90.00)*( hormUp )); // get degree for hormone receiver about upDegree ~ 45*,
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
if (pos_up_end_right > pos_up_end_left)
{
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
step_up_left = stepmin;
}
else if (pos_up_end_right < pos_up_end_left)
{
step_up_right = stepmin;
step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
}
else // step_up_right == step_up_left
{
step_up_right = stepmin;
step_up_left = stepmin;
}
}
/*--------------------------------------------------------------------*/
/* servo in left side */
/*--------------------------------------------------------------------*/
void servoLeft()
{
if(state_count_left == 1)
{
Servo1.SetPosition(pos_down_left); // pos_down_left = 1400.00
wait(waittime); // 0.001 ms
pos_down_left += step_down_left;
if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) // pos_down_end_left ~ 1700
{
state_count_left = 2;
}
}
else if(state_count_left == 2)
{
Servo2.SetPosition(pos_up_left); // pos_up_left = 1000.00
wait(waittime);
pos_up_left += step_up_left;
if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left)
{
state_count_left = 3;
}
}
else if(state_count_left == 3)
{
Servo1.SetPosition(pos_down_left);
wait(waittime);
pos_down_left -= step_down_left;
if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left)
{
state_count_left = 4;
}
}
else if(state_count_left == 4)
{
Servo2.SetPosition(pos_up_left);
wait(waittime);
pos_up_left -= step_up_left;
if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left)
{
state_count_left = 5;
}
}
else if (state_count_left == 5 and round_count_left < round)
{
round_count_left++;
state_count_left = 1;
pos_down_left = pos_down_start;
pos_up_left = pos_up_start;
}
}
/*--------------------------------------------------------------------*/
/* servo in right side */
/*--------------------------------------------------------------------*/
void servoRight()
{
if(state_count_right == 1)
{
Servo3.SetPosition(pos_down_right);
wait(waittime);
pos_down_right = pos_down_right + step_down_right;
if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start)
{
state_count_right = 2;
}
}
else if(state_count_right == 2)
{
Servo4.SetPosition(pos_up_right);
wait(waittime);
pos_up_right = pos_up_right + step_up_right;
if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right)
{
state_count_right = 3;
}
}
else if(state_count_right == 3)
{
Servo3.SetPosition(pos_down_right);
wait(waittime);
pos_down_right = pos_down_right - step_down_right;
if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right)
{
state_count_right = 4;
}
}
else if(state_count_right == 4)
{
Servo4.SetPosition(pos_up_right);
wait(waittime);
pos_up_right = pos_up_right - step_up_right;
if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right)
{
state_count_right = 5;
}
}
else if (state_count_right == 5 and round_count_right < round)
{
round_count_right = round_count_right+1;
state_count_right = 1;
pos_down_right = pos_down_start;
pos_up_right = pos_up_start;
}
}
/*--------------------------------------------------------------------*/
/* check IMU first */
/*--------------------------------------------------------------------*/
bool checkIMUFirst(float SDOfRoll, float SDOfPitch, float SDOfYaw)
{
if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 0)
{
initCheck = 1;
return true;
}
return false;
}
/*--------------------------------------------------------------------*/
/* print state gait for debuging */
/*--------------------------------------------------------------------*/
void printStateGait()
{
if(state_count_left == 1 and state_count_right == 1 and stateGaitOne == 0)
{
//pc.printf("\n\r State Gait 1 \n\r");
pc.printf("\n\rG1\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \n\r");
stateGaitOne = 1; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 0;
}
else if(state_count_left == 2 and state_count_right == 2 and stateGaitTwo == 0)
{
//pc.printf("\n\r State Gait 2 \n\r");
pc.printf("\n\rG2\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \n\r");
stateGaitOne = 0; stateGaitTwo = 1; stateGaitThree = 0; stateGaitFour = 0;
}
else if(state_count_left == 3 and state_count_right == 3 and stateGaitThree == 0)
{
//pc.printf("\n\r State Gait 3 \n\r");
pc.printf("\n\rG3\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \n\r");
stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 1; stateGaitFour = 0;
}
else if(state_count_left == 4 and state_count_right == 4 and stateGaitFour == 0)
{
//pc.printf("\n\r State Gait 4 \n\r");
pc.printf("\n\rG4\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t\t*\t*\t*\t*\t\t \n\r");
stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 1;
}
}
/*--------------------------------------------------------------------*/