TurtleBot
turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of
TurtleBot_v01
by 
worasuchad haomachai
Diff: main.cpp
- Revision:
- 2:18835f8732ad
- Parent:
- 1:13164a15fbf6
- Child:
- 3:5e867483469e
--- a/main.cpp Thu Apr 19 01:54:55 2018 +0000
+++ b/main.cpp Wed Jun 20 04:59:45 2018 +0000
@@ -1,14 +1,24 @@
+//////////////////////////////////////////////////////////////////
+// project: TurtleBot Project //
+// code v.: 0.0 //
+// board : NUCLEO-F303KB //
+// date : 19/6/2018 //
+// code by: Worasuchad Haomachai //
+// detail : Modify IMU thread //
+//////////////////////////////////////////////////////////////////
+
+///////////////////////// init ////////////////////////////////
+//////////////////////////////////////////////////////////////////
#include "mbed.h"
#include "Servo.h"
#include "rtos.h"
#include "attitude.h"
#include "math.h"
-Serial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX); //Serial Port
Timer timer1;
Timer timerwalk;
-
Thread thread1;
Thread thread2;
@@ -17,37 +27,40 @@
Servo Servo3(D9);
Servo Servo4(D10);
+///////////////////////// prototype func ///////////////////////
+//////////////////////////////////////////////////////////////////
+void servo();
void IMU();
+void move();
+/////// In Move //////
+void cal_step_down();
+void cal_step_up();
+void servo_Right();
+//////////////////////
void Cal_si();
void Avg();
void Cal_Cg_linear();
void Cal_Cg_log();
-void move();
void receive_hormone();
-void cal_step_down(); //calculate step
-void cal_step_up();
-void servo();
-void servo_Right();
+float calculateSD(float data[]);
+float HormoneCon(float SiPreProcess);
+
+///////////////////////// variable ///////////////////////////////
+//////////////////////////////////////////////////////////////////
int walking_time;
int i = 0;
-float avg = 0.00;
-float sum = 0.00;
-float roll_data[10];
-float Si = 0.00 ;
-float Cg = 0.00;
float Cg_down = 0.00;
float Cg_up = 0;
-float standardDeviation = 0;
-float sd = 0;
+float Cg = 0.00, CgPrevious = 0.00;
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 = 5;
+float round = 10;
float waittime = 0.001 ;
float pos_down_left = 1400.00;
@@ -68,65 +81,78 @@
float step_up_right;
float step_down_right;
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- //
-int main() { //
- pc.baud(1000000); //
- timer1.start(); // start timer counting //
- pc.printf("PRESS '1'\n"); //
- if (pc.getc() == '1'){ //
- thread2.start(servo); //
- } //
-} //
- //
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-///////////////////////////Control SERVO////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////
-void servo() {
+///////////////////////// main ////////////////////////////
+//////////////////////////////////////////////////////////////////
+int main()
+{
+ pc.baud(115200);
+ timer1.start(); // start timer counting
+ pc.printf("PRESS '1'\n");
+ if (pc.getc() == '1')
+ {
+ thread2.start(servo); // servo thread start
+ }
+ //thread2.start(servo);
+}
+
+///////////////////////// Servo ///////////////////////////
+//////////////////////////////////////////////////////////////////
+void servo()
+{
pc.printf("start\n");
- attitude_setup();
- thread1.start(IMU);
+ attitude_setup(); // IMU setup
+ thread1.start(IMU); // IMU thread start
//wait(0.01);
- timerwalk.start(); // start timer counting
- move();
+ timerwalk.start(); // start timer counting
+ move(); // func of driving servo
//pc.printf("%f\n",roll_min);
//pc.printf("%f\n",roll_max);
}
-void receive_hormone(){
- down_degree = 90.00*(1.00-(0.06*Cg_down));
- /*pc.printf("%f \t",Si);
- pc.printf("down\t");
+///////////////////////// receive_hormone /////////////////
+//////////////////////////////////////////////////////////////////
+void receive_hormone()
+{
+ //down_degree = 90.00f*(1.00f-(0.06f*Cg_down));
+ down_degree = 83.50;
+ /*pc.printf("down\t");
pc.printf("%f \t",down_degree);
pc.printf("%f\t",Cg);*/
- up_degree = 45.00*(1.00+(0.7*Cg_up));
+ //up_degree = 45.00f*(1.00f+(0.7f*Cg_up));
+ up_degree = 45.00;
/*pc.printf("up\t");
pc.printf("%f \t",up_degree);
- pc.printf("%f\n",Cg);*/
- if(down_degree < 85){
+ pc.printf("%f\n",Cg);
+ if(down_degree < 85)
+ {
down_degree = 85;
- if(up_degree > 75){
- up_degree = 75;
+ if(up_degree > 75)
+ {
+ up_degree = 75;
}
- }
+ }*/
}
-void cal_step_down(){
+///////////////////////// cal_step_down ///////////////////
+//////////////////////////////////////////////////////////////////
+void cal_step_down()
+{
//pc.printf("down");
//pc.printf("%f \n",down_degree);
- pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree)));
- pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree)));
- 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);
+ pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); // get degree for hormone receiver about down_degree ~ 90*,
+ pos_down_end_right = (1070.00 + ((700.00/90.00)*(down_degree))); // 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){
+ }
+ 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{
+ }
+ else // pos_down_end_right == pos_down_end_left
+ {
step_down_right = stepmin;
step_down_left = stepmin;
}
@@ -139,19 +165,27 @@
pc.printf("step_down_left");
pc.printf("%f\n",step_down_left);*/
}
-
-void cal_step_up(){
+
+///////////////////////// cal_step_up /////////////////////
+//////////////////////////////////////////////////////////////////
+void cal_step_up()
+{
//pc.printf("up");
//pc.printf("%f \n",up_degree);
- pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree));
- pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree));
- 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);
+ pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); // get degree for hormone receiver about up_degree ~ 45*,
+ pos_up_end_right = 1000.00 + ((700.00/90.00)*(up_degree)); // 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){
+ }
+ 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{
+ }
+ else // step_up_right == step_up_left
+ {
step_up_right = stepmin;
step_up_left = stepmin;
}
@@ -162,15 +196,17 @@
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("%f\n",step_up_left);*/
}
-
-void move(){
+
+///////////////////////// move ////////////////////////////
+//////////////////////////////////////////////////////////////////
+void move()
+{
Servo1.Enable(1000,20000);
Servo2.Enable(1000,20000);
Servo3.Enable(1000,20000);
- Servo4.Enable(1000,20000);
-
+ Servo4.Enable(1000,20000);
/*pc.printf("Si\t");
pc.printf("%f \t",Si);
pc.printf("down\t");
@@ -179,116 +215,148 @@
pc.printf("up\t");
pc.printf("%f \t",up_degree);
pc.printf("%f\n",Cg);*/
- while(1) {
- cal_step_down();
- cal_step_up();
- servo_Right();
- if(state_count_left == 1) {
- Servo1.SetPosition(pos_down_left);
- wait(waittime);
+ 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
+ {
+ Servo1.SetPosition(pos_down_left); // pos_down_left = 1400.00
+ wait(waittime); // 0.001 ms
pos_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) {
+ 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;
}
/*pc.printf("LAD");
pc.printf("%f\n",pos_down_left);
pc.printf("LAP");
pc.printf("%f\n",pos_up_left);*/
- } else if(state_count_left == 2) {
- Servo2.SetPosition(pos_up_left);
+ }
+ else if(state_count_left == 2)
+ {
+ Servo2.SetPosition(pos_up_left); // pos_up_left = 1000.00
wait(waittime);
pos_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) {
+ 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;
}
/*pc.printf("LBD");
pc.printf("%f\n",pos_down_left);
pc.printf("LBP");
pc.printf("%f\n",pos_up_left);*/
- } else if(state_count_left == 3) {
+ }
+ else if(state_count_left == 3)
+ {
Servo1.SetPosition(pos_down_left);
wait(waittime);
pos_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) {
+ 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;
}
/*pc.printf("LCD");
pc.printf("%f\n",pos_down_left);
pc.printf("LCP");
pc.printf("%f\n",pos_up_left);*/
- } else if(state_count_left == 4) {
+ }
+ else if(state_count_left == 4)
+ {
Servo2.SetPosition(pos_up_left);
wait(waittime);
pos_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) {
+ if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left)
+ {
state_count_left = 0;
}
/*pc.printf("LDD");
pc.printf("%f\n",pos_down_left);
pc.printf("LDP");
pc.printf("%f\n",pos_up_left);*/
- } else if (state_count_left == 0 and round_count_left < round) {
+ }
+ else if (state_count_left == 0 and round_count_left < round)
+ {
round_count_left = round_count_left+1;
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){
+ }
+ 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");
+ //pc.printf("Finish \n");
walking_time = timerwalk.read_ms();
- pc.printf("Walking time = %d \n", walking_time);
+ //pc.printf("Walking time = %d \n", walking_time);
break;
}
}
}
-
+
+///////////////////////// servo_Right /////////////////////
+//////////////////////////////////////////////////////////////////
void servo_Right()
{
- if(state_count_right == 1) {
+ 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) {
+ if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start)
+ {
state_count_right = 2;
}
/*pc.printf("RAD");
pc.printf("%f\n",pos_down_right);
pc.printf("RAP");
pc.printf("%f\n",pos_up_right);*/
- } else if(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) {
+ 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;
}
/*pc.printf("RBD");
pc.printf("%f\n",pos_down_right);
pc.printf("RBP");
pc.printf("%f\n",pos_up_right);*/
- } else if(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) {
+ 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;
}
/*pc.printf("RCD");
pc.printf("%f\n",pos_down_right);
pc.printf("RCP");
pc.printf("%f\n",pos_up_right);*/
- } else if(state_count_right == 4) {
+ }
+ else if(state_count_right == 4)
+ {
Servo4.SetPosition(pos_up_right);
- wait(waittime);
+ 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) {
+ if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right)
+ {
state_count_right = 0;
}
/*pc.printf("RDD");
pc.printf("%f\n",pos_down_right);
pc.printf("RDP");
pc.printf("%f\n",pos_up_right);*/
- } else if (state_count_right == 0 and round_count_right < round) {
+ }
+ else if (state_count_right == 0 and round_count_right < round)
+ {
round_count_right = round_count_right+1;
state_count_right = 1;
pos_down_right = pos_down_start;
@@ -297,17 +365,22 @@
}
}
-///////////////////////////Control IMU////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////
-void IMU(){
- pc.printf("roll\t");
+
+///////////////////////// 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() >=10000)// read time in ms
+ pc.printf("up\n"); */
+ while(1)
+ {
+ if (timer1.read_us() >= 1000) // read time in 1 ms
{
attitude_get();
@@ -315,70 +388,80 @@
//pc.printf(" %f \t", ay*10 );
//pc.printf(" %f \t", az*10-10); //cm/s*s
- //pc.printf("%f\t %.0f \t %.0f \n\r", roll, pitch, yaw);
-
+ 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);
-
- timer1.reset(); // reset timer
- Cal_si();
+
+ ////////////////////////// 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
}
}
}
-void Cal_si(){
- if(state_count_left == 4 or state_count_right == 4){
- roll_data[i] = roll;
- //pc.printf("%f\n",roll_data[i]);
- Avg();
- //pc.printf("Avg ");
- //pc.printf("%f\n",avg);
- Cal_Cg_linear();
- //pc.printf("roll\t");
- pc.printf("%f\t",roll_data[i]);
- //pc.printf("Si\t");
- pc.printf("%f\t",Si);
- //pc.printf("Cg\t");
- pc.printf("%f\t",Cg);
- //pc.printf("down\t");
- pc.printf("%f \t",down_degree);
- //pc.printf("up\t");
- pc.printf("%f \n",up_degree);
- if(i == 9){
- i = 0;
- }else{
- i = i+1;
- }
- }
+///////////////// Hormone Concentration /////////////////////
+//////////////////////////////////////////////////////////////////
+float HormoneCon(float SiPreProcess)
+{
+ float CgTemp;
+
+ //pc.printf("SiPreProcess: %.3f\t", SiPreProcess);
+ //pc.printf("CgPrevious: %.3f\t", CgPrevious);
+
+ ////// hormone gland //////
+ CgTemp = (0.8f*SiPreProcess) + (0.5f*CgPrevious);
+ //pc.printf("CgTemp: %.3f\t", CgTemp);
+ Cg = 1/( 1+exp(-CgTemp) ); // used sigmoid func for calculating Cg which much have value between 0 - 1
+ ///////////////////////////
+
+ //pc.printf("First Term: %.3f\t", (float)(0.8f*SiPreProcess));
+ //pc.printf("Secound Term: %.3f\t", (float)(0.5f*CgPrevious));
+ CgPrevious = Cg;
+ //*********//
+ Cg_down = Cg;
+ Cg_up = Cg;
+
+ return Cg;
}
-void Avg(){
- sum = sum + roll;
- avg = sum/10;
- standardDeviation = (roll - avg)*(roll-avg);
- sd = sqrt(standardDeviation/10);
- if(avg < 0){
- avg = avg*(-1);
+///////////////////////// calculateSD /////////////////////
+//////////////////////////////////////////////////////////////////
+float calculateSD(float sdData[])
+{
+ float sum = 0.0, mean, standardDeviation = 0.0;
+ int i;
+
+ for(i = 0; i < 10 ; i++)
+ {
+ sum += sdData[i];
+ //pc.printf("sum = %.2f \n\r",sum);
+ }
+ mean = sum/10;
+ //pc.printf("mean = %.2f \n\r",mean);
+
+ for(i = 0; i < 10; i++)
+ {
+ standardDeviation += pow(sdData[i] - mean, 2);
+ //pc.printf("standardDeviation = %.2f \n\r",standardDeviation);
}
-}
-
-
-
-void Cal_Cg_linear(){
- Si = sd/0.6;
- sum = sum - roll_data[i];
- if(Si > 0){
- Cg = (0.8*Si)+(0.5*Cg);
- if(Cg > 1){
- Cg = 1;
- }
- }else{
- Cg = 0.00;
- }
- Cg_down = 1/0.1*exp(2*(Cg-2.081))-0.15;
- Cg_up = (0.8*log(Cg+0.4))+0.734;
+ return sqrt(standardDeviation / 10);
}
\ No newline at end of file