worasuchad haomachai
Update hormone config
Dependencies: mbed Servo PM mbed-rtos hormone calculator
Diff: main.cpp
- Revision:
- 3:5e867483469e
- Parent:
- 2:18835f8732ad
- Child:
- 4:ec7e68b84f2b
--- a/main.cpp Wed Jun 20 04:59:45 2018 +0000
+++ b/main.cpp Thu Jun 21 06:05:00 2018 +0000
@@ -29,38 +29,39 @@
///////////////////////// prototype func ///////////////////////
//////////////////////////////////////////////////////////////////
+void IMU();
void servo();
-void IMU();
-void move();
-/////// In Move //////
+/////// In servo //////
void cal_step_down();
void cal_step_up();
+void servo_Left();
void servo_Right();
//////////////////////
-void Cal_si();
-void Avg();
-void Cal_Cg_linear();
-void Cal_Cg_log();
void receive_hormone();
float calculateSD(float data[]);
+float calculateMean(float meanData[]);
float HormoneCon(float SiPreProcess);
+bool directionRobot(float directionMean, float directionSD);
///////////////////////// variable ///////////////////////////////
//////////////////////////////////////////////////////////////////
+// home param
int walking_time;
-int i = 0;
+int initDirection = 0;
+// hormone param
float Cg_down = 0.00;
float Cg_up = 0;
float Cg = 0.00, CgPrevious = 0.00;
-
+
+// servo motor param
float pos_down_start = 1400.00;
float pos_up_start = 1000.00;
float down_degree = 90.00;
float up_degree = 45.00;
float stepmin = 1;
-float round = 10;
+float round = 5;
float waittime = 0.001 ;
float pos_down_left = 1400.00;
@@ -86,28 +87,129 @@
int main()
{
pc.baud(115200);
- timer1.start(); // start timer counting
- pc.printf("PRESS '1'\n");
+ timer1.start(); // start timer counting
+ attitude_setup(); // IMU setup
+ //thread1.start(IMU); // IMU thread start
+ pc.printf(" Please press! '1' to start..\n\r");
if (pc.getc() == '1')
- {
- thread2.start(servo); // servo thread start
+ {
+ thread1.start(IMU); // IMU thread start
+ //thread2.start(servo); // servo thread start
+ }
+}
+
+///////////////////////// IMU /////////////////////////////
+//////////////////////////////////////////////////////////////////
+void IMU()
+{
+ int i;
+ float ArrayOfRoll[10], ArrayOfYaw[10];
+ float SDOfRoll, SDOfYaw, MeanOfYaw, directOfRobot = 0.00;
+/* pc.printf("roll\t");
+ pc.printf("Si\t");
+ pc.printf("Cg\t");
+ pc.printf("down\t");
+ pc.printf("up\n"); */
+ while(1)
+ {
+ if (timer1.read_us() >= 1000) // read time in 1 ms
+ {
+ attitude_get();
+
+ //pc.printf(" %f \t", ax*10 );
+ //pc.printf(" %f \t", ay*10 );
+ //pc.printf(" %f \t", az*10-10); //cm/s*s
+
+ //pc.printf("%f\t %f\t %f\n\r", roll, pitch, yaw);
+ pc.printf("%f\n\r",yaw);
+ //pc.printf("up\t");
+ //pc.printf("%f \t",up_degree);
+ //pc.printf("%f\t",Cg);
+ //pc.printf("down\t");
+ //pc.printf("%f \t",down_degree);
+ //pc.printf("%f\n",Cg);
+
+ ////////////////////////// Signal Pre-Process every 10 ms //////////////////////
+ ////////////////////////////////////////////////////////////////////////////////
+ if(i < 10)
+ {
+ ArrayOfRoll[i] = roll;
+ ArrayOfYaw[i] = yaw;
+ //pc.printf("i = %i ,ArrayOfRoll = %.2f, roll= %.2f\n\r",i, ArrayOfRoll[i], roll);
+ i++;
+ }
+ else
+ {
+ //////////// roll //////////////
+ //pc.printf("Roll - Standard Deviation = %.2f \n\r", calculateSD(ArrayOfRoll)); // every 10 ms
+ SDOfRoll = calculateSD(ArrayOfRoll);
+ //pc.printf("HC: %.3f\n\r", HormoneCon(SDOfRoll));
+ HormoneCon(SDOfRoll);
+
+ //////////// yaw ///////////////
+ MeanOfYaw = calculateMean(ArrayOfYaw);
+ pc.printf("MeanOfYaw: %.3f\n\r", MeanOfYaw);
+ SDOfYaw = calculateSD(ArrayOfYaw);
+ pc.printf("SDOfYaw: %.3f\n\r", SDOfYaw);
+
+ if(directionRobot(MeanOfYaw, SDOfYaw)) // only one time for comming
+ {
+ directOfRobot = MeanOfYaw;
+ thread2.start(servo); // Servo Thread
+ pc.printf("::::::::::::::::::::::::::: Init Direction OK :::::::::::::::::::::::::::\n\r");
+ }
+
+ pc.printf("directOfRobot: %.3f\n\r", directOfRobot);
+ // func find diff beween directOfRobot and MeanOfYaw
+ pc.printf("Diff from direction: %.3f\n\r", abs(directOfRobot - MeanOfYaw));
+
+ // reset iteration
+ i = 0;
+ }
+
+ timer1.reset(); // reset timer
+ }
}
- //thread2.start(servo);
-}
-
-///////////////////////// Servo ///////////////////////////
-//////////////////////////////////////////////////////////////////
-void servo()
+}
+
+///////////////////////// servo ///////////////////////////
+//////////////////////////////////////////////////////////////////
+void servo()
{
- pc.printf("start\n");
- attitude_setup(); // IMU setup
- thread1.start(IMU); // IMU thread start
- //wait(0.01);
- timerwalk.start(); // start timer counting
- move(); // func of driving servo
- //pc.printf("%f\n",roll_min);
- //pc.printf("%f\n",roll_max);
-}
+ Servo1.Enable(1000,20000);
+ Servo2.Enable(1000,20000);
+ Servo3.Enable(1000,20000);
+ Servo4.Enable(1000,20000);
+
+ pc.printf("Servo Start Ja!!!\n");
+ timerwalk.start(); // start timer counting
+ /*pc.printf("Si\t");
+ pc.printf("%f \t",Si);
+ pc.printf("down\t");
+ pc.printf("%f \t",down_degree);
+ pc.printf("%f\t",Cg);
+ pc.printf("up\t");
+ pc.printf("%f \t",up_degree);
+ pc.printf("%f\n",Cg);*/
+ while(1)
+ {
+ receive_hormone();
+ cal_step_down(); // return "step_down_right" and "step_down_left"
+ cal_step_up(); // return "step_up_right" and "step_up_left"
+ servo_Left(); // control left lag
+ servo_Right(); // control right leg
+
+ // fin for walking
+ if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round)
+ {
+ thread1.terminate();
+ pc.printf("Finish! \t");
+ walking_time = timerwalk.read_ms();
+ pc.printf("Walking time = %d ms\n\r", walking_time);
+ break;
+ }
+ }
+}
///////////////////////// receive_hormone /////////////////
//////////////////////////////////////////////////////////////////
@@ -156,14 +258,14 @@
step_down_right = stepmin;
step_down_left = stepmin;
}
- /*pc.printf("pos_down_right");
- pc.printf("%f\n",pos_down_end_right);
- pc.printf("pos_down_left");
- pc.printf("%f\n",pos_down_end_left);
- pc.printf("step_down_right");
- pc.printf("%f\n",step_down_right);
- pc.printf("step_down_left");
- pc.printf("%f\n",step_down_left);*/
+ /*pc.printf("pos_down_right: ");
+ pc.printf("%f\t\t",pos_down_end_right);
+ pc.printf("pos_down_left: ");
+ pc.printf("%f\t\t",pos_down_end_left);
+ pc.printf("step_down_right: ");
+ pc.printf("%f\t\t",step_down_right);
+ pc.printf("step_down_left: ");
+ pc.printf("%f\n\r",step_down_left);*/
}
///////////////////////// cal_step_up /////////////////////
@@ -189,43 +291,25 @@
step_up_right = stepmin;
step_up_left = stepmin;
}
- /*pc.printf("pos_up_right");
- pc.printf("%f\n",pos_up_end_right);
- pc.printf("pos_up_left");
- pc.printf("%f\n",pos_up_end_left);
- pc.printf("step_up_right");
- pc.printf("%f\n",step_up_right);;
- pc.printf("step_up_left");
- pc.printf("%f\n",step_up_left);*/
+ /*pc.printf("pos_up_right: ");
+ pc.printf("%f\t\t",pos_up_end_right);
+ pc.printf("pos_up_left: ");
+ pc.printf("%f\t\t",pos_up_end_left);
+ pc.printf("step_up_right: ");
+ pc.printf("%f\t\t",step_up_right);;
+ pc.printf("step_up_left: ");
+ pc.printf("%f\n\r",step_up_left);*/
}
-///////////////////////// move ////////////////////////////
-//////////////////////////////////////////////////////////////////
-void move()
+///////////////////////// servo_Left //////////////////////
+//////////////////////////////////////////////////////////////////
+void servo_Left()
{
- Servo1.Enable(1000,20000);
- Servo2.Enable(1000,20000);
- Servo3.Enable(1000,20000);
- Servo4.Enable(1000,20000);
- /*pc.printf("Si\t");
- pc.printf("%f \t",Si);
- pc.printf("down\t");
- pc.printf("%f \t",down_degree);
- pc.printf("%f\t",Cg);
- pc.printf("up\t");
- pc.printf("%f \t",up_degree);
- pc.printf("%f\n",Cg);*/
- while(1)
- {
- cal_step_down(); // return "step_down_right" and "step_down_left"
- cal_step_up(); // return "step_up_right" and "step_up_left"
- servo_Right(); // control right leg
-
- if(state_count_left == 1) // control left lag
+ if(state_count_left == 1)
{
Servo1.SetPosition(pos_down_left); // pos_down_left = 1400.00
wait(waittime); // 0.001 ms
- pos_down_left = pos_down_left + step_down_left;
+ 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;
@@ -239,7 +323,7 @@
{
Servo2.SetPosition(pos_up_left); // pos_up_left = 1000.00
wait(waittime);
- pos_up_left = pos_up_left + step_up_left;
+ 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;
@@ -253,7 +337,7 @@
{
Servo1.SetPosition(pos_down_left);
wait(waittime);
- pos_down_left = pos_down_left - step_down_left;
+ 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;
@@ -267,7 +351,7 @@
{
Servo2.SetPosition(pos_up_left);
wait(waittime);
- pos_up_left = pos_up_left - step_up_left;
+ 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 = 0;
@@ -279,20 +363,11 @@
}
else if (state_count_left == 0 and round_count_left < round)
{
- round_count_left = round_count_left+1;
+ round_count_left++;
state_count_left = 1;
pos_down_left = pos_down_start;
pos_up_left = pos_up_start;
}
- else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round)
- {
- thread1.terminate();
- //pc.printf("Finish \n");
- walking_time = timerwalk.read_ms();
- //pc.printf("Walking time = %d \n", walking_time);
- break;
- }
- }
}
///////////////////////// servo_Right /////////////////////
@@ -361,60 +436,6 @@
state_count_right = 1;
pos_down_right = pos_down_start;
pos_up_right = pos_up_start;
- receive_hormone();
- }
-}
-
-
-///////////////////////// IMU /////////////////////////////
-//////////////////////////////////////////////////////////////////
-void IMU()
-{
- int i;
- float SiInput[10];
- float SDOfRoll;
-/* pc.printf("roll\t");
- pc.printf("Si\t");
- pc.printf("Cg\t");
- pc.printf("down\t");
- pc.printf("up\n"); */
- while(1)
- {
- if (timer1.read_us() >= 1000) // read time in 1 ms
- {
- attitude_get();
-
- //pc.printf(" %f \t", ax*10 );
- //pc.printf(" %f \t", ay*10 );
- //pc.printf(" %f \t", az*10-10); //cm/s*s
-
- pc.printf("%f\t %f\t %f\n\r", roll, pitch, yaw);
- //pc.printf("up\t");
- //pc.printf("%f \t",up_degree);
- //pc.printf("%f\t",Cg);
- //pc.printf("down\t");
- //pc.printf("%f \t",down_degree);
- //pc.printf("%f\n",Cg);
-
- ////////////////////////// Signal Pre-Process every 10 ms //////////////////////
- ////////////////////////////////////////////////////////////////////////////////
- if(i < 10)
- {
- SiInput[i] = roll;
- //pc.printf("i = %i ,SiInput = %.2f, roll= %.2f\n\r",i, SiInput[i], roll);
- i++;
- }
- else
- {
- //pc.printf("Roll - Standard Deviation = %.2f \n\r", calculateSD(SiInput)); // every 10 ms
- SDOfRoll = calculateSD(SiInput);
- //pc.printf("HC: %.3f\n\r", HormoneCon(SDOfRoll));
-
- i = 0;
- }
-
- timer1.reset(); // reset timer
- }
}
}
@@ -450,7 +471,7 @@
float sum = 0.0, mean, standardDeviation = 0.0;
int i;
- for(i = 0; i < 10 ; i++)
+ for(i = 0; i < 10 ; ++i)
{
sum += sdData[i];
//pc.printf("sum = %.2f \n\r",sum);
@@ -458,10 +479,39 @@
mean = sum/10;
//pc.printf("mean = %.2f \n\r",mean);
- for(i = 0; i < 10; i++)
+ for(i = 0; i < 10; ++i)
{
standardDeviation += pow(sdData[i] - mean, 2);
//pc.printf("standardDeviation = %.2f \n\r",standardDeviation);
}
return sqrt(standardDeviation / 10);
+}
+
+///////////////////////// calculateMean ///////////////////
+//////////////////////////////////////////////////////////////////
+float calculateMean(float meanData[])
+{
+ float sum = 0.0, mean;
+ int i;
+
+ for(i = 0; i < 10 ; ++i)
+ {
+ sum += meanData[i];
+ //pc.printf("sum = %.2f \n\r",sum);
+ }
+ mean = sum/10;
+ //pc.printf("mean = %.2f \n\r",mean);
+ return mean;
+}
+
+///////////////////////// directionRobot //////////////////
+//////////////////////////////////////////////////////////////////
+bool directionRobot(float directionMean, float directionSD)
+{
+ if( directionSD < 0.06f and initDirection == 0 )
+ {
+ initDirection++;
+ return 1;
+ }
+ return 0;
}
\ No newline at end of file