TurtleBot
turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of
TurtleBot_v01
by 
worasuchad haomachai
Diff: main.cpp
- Revision:
- 0:43d21d5145d3
- Child:
- 1:13164a15fbf6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 10 13:42:04 2018 +0000
@@ -0,0 +1,338 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "rtos.h"
+#include "attitude.h"
+
+Serial pc(USBTX, USBRX);
+
+Timer timer1;
+Timer timerwalk;
+
+Thread thread1;
+Thread thread2;
+
+Servo Servo1(D6);
+Servo Servo2(D8);
+Servo Servo3(D9);
+Servo Servo4(D10);
+
+void IMU();
+void Cal_si();
+void Avg();
+void Cal_Cg();
+void move();
+void receive_hormone();
+void cal_step_down(); //calculate step
+void cal_step_up();
+void servo();
+void servo_Right();
+
+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 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 = 200;
+float waittime = 0.001 ;
+
+float pos_down_left = 1400.00;
+float pos_up_left = 1000.00;
+float pos_down_end_left;
+float pos_up_end_left;
+float state_count_left = 1;
+float round_count_left = 1;
+float step_down_left;
+float step_up_left;
+
+float pos_down_right = 1400.00;
+float pos_up_right = 1000.00;
+float pos_down_end_right;
+float pos_up_end_right;
+float state_count_right = 1;
+float round_count_right = 1;
+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() {
+ pc.printf("start\n");
+ attitude_setup();
+ thread1.start(IMU);
+ //wait(0.01);
+ timerwalk.start(); // start timer counting
+ move();
+ //pc.printf("%f\n",roll_min);
+ //pc.printf("%f\n",roll_max);
+}
+
+void receive_hormone(){
+ down_degree = 90.00*(1.00-(0.5*Cg));
+ up_degree = 45.00*(1.00+(0.7*Cg));
+ if(down_degree < 80){
+ down_degree = 80;
+ if(up_degree > 75){
+ up_degree = 75;
+ }
+ }
+}
+
+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);
+ 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{
+ 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);*/
+}
+
+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);
+ 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 = 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); */
+}
+
+void move(){
+ Servo1.Enable(1000,2000);
+ Servo2.Enable(1000,2000);
+ Servo3.Enable(1000,2000);
+ Servo4.Enable(1000,2000);
+ while(1) {
+ cal_step_down();
+ cal_step_up();
+ servo_Right();
+ if(state_count_left == 1) {
+ Servo1.SetPosition(pos_down_left);
+ wait(waittime);
+ 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) {
+ 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);
+ 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) {
+ 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) {
+ 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) {
+ 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) {
+ 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) {
+ 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) {
+ round_count_left = round_count_left+1;
+ state_count_left = 1;
+ pos_down_left = pos_down_start;
+ pos_up_left = pos_up_start;
+ //receive_hormone();
+ } 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;
+ }
+ }
+}
+
+void servo_Right()
+{
+ 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;
+ }
+ /*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) {
+ 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;
+ }
+ /*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) {
+ 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;
+ }
+ /*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) {
+ 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 = 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) {
+ round_count_right = round_count_right+1;
+ state_count_right = 1;
+ pos_down_right = pos_down_start;
+ pos_up_right = pos_up_start;
+ receive_hormone();
+ }
+}
+
+///////////////////////////Control IMU////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+void IMU(){
+ while(1) {
+ if (timer1.read_us() >=10000)// read time in 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 %.0f \t %.0f \n\r", roll, pitch, yaw);
+
+ timer1.reset(); // reset timer
+ Cal_si();
+ }
+ }
+}
+
+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);
+ Si = avg/4;
+ sum = sum - roll_data[i];
+ //pc.printf("Si ");
+ //pc.printf("%f\n",Si);
+ Cal_Cg();
+ //pc.printf("Cg ");
+ //pc.printf("%f\n",Cg);
+ if(i == 9){
+ i = 0;
+ }else{
+ i = i+1;
+ }
+ }
+}
+
+void Avg(){
+ sum = sum + roll;
+ avg = sum/10;
+ if(avg < 0){
+ avg = avg*(-1);
+ }
+}
+
+void Cal_Cg(){
+ if(Si > 0){
+ Cg = (0.9*Si)+(0.3*Cg);
+ if(Cg > 1){
+ Cg = 1;
+ }
+ }else{
+ Cg = 0.00;
+ }
+}
\ No newline at end of file