latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V555 by
main.cpp
- Committer:
- 59010050
- Date:
- 2018-04-02
- Revision:
- 2:436ed0069b61
- Parent:
- 1:852156b5cca1
- Child:
- 3:98ef5105926e
File content as of revision 2:436ed0069b61:
#include "mbed.h" #include "Servo.h" #include "rtos.h" #include "attitude.h" Serial pc(USBTX, USBRX); //Serial bt(A7,A2); Timer timer1; Thread thread1; Thread thread2; void IMU() { while(1) { if (timer1.read_us() >=1000)// 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", gx ); pc.printf(" %f \t", gy ); pc.printf(" %f \t", gz ); //deg/s */ pc.printf("%.0f\t %.0f \t \n\r", roll, pitch ); timer1.reset(); // reset timer } } } Servo Servo1(D6); Servo Servo2(D8); Servo Servo3(D9); Servo Servo4(D10); void servoleft(); void move(); int pos_down = 1400; int pos_up = 1000; int pos_down_end = 1700; int pos_up_end = 1500; int edit_up = 1400; int state_count = 1; int state_count2 = 1; int round_count = 1; int round_count2 = 1; int main() { pc.baud(1000000); //pc.printf("malin"); timer1.start(); // start timer counting if (pc.getc() == '1') { thread2.start(move); //thread1.start(IMU); } } void servoleft(){ if(state_count2 == 1){ Servo1.SetPosition(pos_down); //pos_down = pos_down+5; wait(0.01); pc.printf("left %d\n",pos_down); if(pos_down == pos_down_end+5 and pos_up == 1000){ state_count2 = 2; } } else if(state_count2 == 2){ Servo2.SetPosition(pos_up); //pos_up = pos_up+5; wait(0.01); pc.printf("stage2"); if(pos_down == pos_down_end+5 and pos_up == edit_up+5){ state_count2 = 3; } } else if(state_count2 == 3){ Servo1.SetPosition(pos_down); //pos_down = pos_down-5; wait(0.01); pc.printf("stage3"); if(pos_down == 1395 and pos_up == edit_up+5){ state_count2 = 4; } } else if(state_count2 == 4){ Servo2.SetPosition(pos_up); //pos_up = pos_up-5; wait(0.01); pc.printf("stage4"); if(pos_down == 1395 and pos_up == 995){ state_count2 = 0; } } else if (state_count2 == 0 and round_count2 < 10){ round_count2 = round_count2+1; state_count2 = 1; pos_down = 1400; pos_up = 1000; } else { pc.printf("1stop"); //thread1.stop(IMU); } } void move() { attitude_setup(); Servo1.Enable(1000,2000); Servo2.Enable(1000,2000); Servo3.Enable(1000,2000); Servo4.Enable(1000,2000); //pc.printf("start"); //pc.printf("start"); //pc.printf("start"); //pc.printf("start"); pc.printf("start"); while(1){ servoleft(); if(state_count == 1){ Servo3.SetPosition(pos_down); pos_down = pos_down+5; wait(0.01); pc.printf("right %d\n",pos_down); if(pos_down == pos_down_end+5 and pos_up == 1000){ state_count = 2; } } else if(state_count == 2){ Servo4.SetPosition(pos_up); pos_up = pos_up+5; wait(0.01); if(pos_down == pos_down_end+5 and pos_up == pos_up_end+5){ state_count = 3; } } else if(state_count == 3){ Servo3.SetPosition(pos_down); pos_down = pos_down-5; wait(0.01); if(pos_down == 1395 and pos_up == pos_up_end+5){ state_count = 4; } } else if(state_count == 4){ Servo4.SetPosition(pos_up); pos_up = pos_up-5; wait(0.01); if(pos_down == 1395 and pos_up == 995){ state_count = 0; } } else if (state_count == 0 and round_count < 10){ round_count = round_count+1; state_count = 1; pos_down = 1400; pos_up = 1000; } else { pc.printf("stop"); } } }