TurtleBot / Mbed 2 deprecated TurtleBot_V5-2

latest

Dependencies:   Servo mbed-rtos mbed

Fork of TurtleBot_V555 by Sopitchaya Lummaetee

main.cpp@2:436ed0069b61, 2018-04-02 (annotated)

Committer:
59010050
Date:
Mon Apr 02 11:46:08 2018 +0000
Revision:
2:436ed0069b61
Parent:
1:852156b5cca1
Child:
3:98ef5105926e
mon

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Khanchana 0:3af6fae90816 1 #include "mbed.h"
Khanchana 0:3af6fae90816 2 #include "Servo.h"
Khanchana 0:3af6fae90816 3 #include "rtos.h"
59010050 1:852156b5cca1 4 #include "attitude.h"
Khanchana 0:3af6fae90816 5
Khanchana 0:3af6fae90816 6 Serial pc(USBTX, USBRX);
59010050 1:852156b5cca1 7 //Serial bt(A7,A2);
59010050 1:852156b5cca1 8 Timer timer1;
59010050 1:852156b5cca1 9 Thread thread1;
59010050 1:852156b5cca1 10
59010050 1:852156b5cca1 11 Thread thread2;
59010050 1:852156b5cca1 12
59010050 1:852156b5cca1 13 void IMU()
59010050 1:852156b5cca1 14 {
59010050 1:852156b5cca1 15 while(1) {
59010050 1:852156b5cca1 16 if (timer1.read_us() >=1000)// read time in ms
59010050 1:852156b5cca1 17 {
59010050 1:852156b5cca1 18 attitude_get();
59010050 1:852156b5cca1 19 pc.printf(" %f \t", ax*10 );
59010050 1:852156b5cca1 20 pc.printf(" %f \t", ay*10 );
59010050 1:852156b5cca1 21 pc.printf(" %f \t", az*10 - 10); //cm/s*s
59010050 1:852156b5cca1 22
59010050 1:852156b5cca1 23 pc.printf(" %f \t", gx );
59010050 1:852156b5cca1 24 pc.printf(" %f \t", gy );
59010050 1:852156b5cca1 25 pc.printf(" %f \t", gz ); //deg/s */
59010050 1:852156b5cca1 26 pc.printf("%.0f\t %.0f \t \n\r", roll, pitch );
59010050 1:852156b5cca1 27
59010050 1:852156b5cca1 28 timer1.reset(); // reset timer
59010050 1:852156b5cca1 29 }
59010050 1:852156b5cca1 30 }
59010050 1:852156b5cca1 31 }
59010050 1:852156b5cca1 32
Khanchana 0:3af6fae90816 33 Servo Servo1(D6);
Khanchana 0:3af6fae90816 34 Servo Servo2(D8);
Khanchana 0:3af6fae90816 35 Servo Servo3(D9);
Khanchana 0:3af6fae90816 36 Servo Servo4(D10);
Khanchana 0:3af6fae90816 37
59010050 2:436ed0069b61 38 void servoleft();
59010050 1:852156b5cca1 39 void move();
Khanchana 0:3af6fae90816 40
Khanchana 0:3af6fae90816 41 int pos_down = 1400;
Khanchana 0:3af6fae90816 42 int pos_up = 1000;
Khanchana 0:3af6fae90816 43 int pos_down_end = 1700;
59010050 2:436ed0069b61 44 int pos_up_end = 1500;
59010050 2:436ed0069b61 45 int edit_up = 1400;
59010050 2:436ed0069b61 46
59010050 1:852156b5cca1 47 int state_count = 1;
59010050 2:436ed0069b61 48 int state_count2 = 1;
59010050 1:852156b5cca1 49 int round_count = 1;
59010050 2:436ed0069b61 50 int round_count2 = 1;
Khanchana 0:3af6fae90816 51 int main() {
59010050 1:852156b5cca1 52 pc.baud(1000000);
59010050 2:436ed0069b61 53 //pc.printf("malin");
59010050 1:852156b5cca1 54
59010050 1:852156b5cca1 55 timer1.start(); // start timer counting
59010050 1:852156b5cca1 56 if (pc.getc() == '1')
59010050 1:852156b5cca1 57 {
59010050 1:852156b5cca1 58 thread2.start(move);
59010050 2:436ed0069b61 59 //thread1.start(IMU);
59010050 1:852156b5cca1 60 }
59010050 1:852156b5cca1 61
59010050 1:852156b5cca1 62 }
59010050 2:436ed0069b61 63 void servoleft(){
59010050 2:436ed0069b61 64
59010050 2:436ed0069b61 65 if(state_count2 == 1){
59010050 2:436ed0069b61 66 Servo1.SetPosition(pos_down);
59010050 2:436ed0069b61 67 //pos_down = pos_down+5;
59010050 2:436ed0069b61 68 wait(0.01);
59010050 2:436ed0069b61 69 pc.printf("left %d\n",pos_down);
59010050 2:436ed0069b61 70 if(pos_down == pos_down_end+5 and pos_up == 1000){
59010050 2:436ed0069b61 71 state_count2 = 2;
59010050 2:436ed0069b61 72 }
59010050 2:436ed0069b61 73 }
59010050 2:436ed0069b61 74 else if(state_count2 == 2){
59010050 2:436ed0069b61 75 Servo2.SetPosition(pos_up);
59010050 2:436ed0069b61 76 //pos_up = pos_up+5;
59010050 2:436ed0069b61 77 wait(0.01);
59010050 2:436ed0069b61 78 pc.printf("stage2");
59010050 2:436ed0069b61 79 if(pos_down == pos_down_end+5 and pos_up == edit_up+5){
59010050 2:436ed0069b61 80 state_count2 = 3;
59010050 2:436ed0069b61 81 }
59010050 2:436ed0069b61 82 }
59010050 2:436ed0069b61 83 else if(state_count2 == 3){
59010050 2:436ed0069b61 84 Servo1.SetPosition(pos_down);
59010050 2:436ed0069b61 85 //pos_down = pos_down-5;
59010050 2:436ed0069b61 86 wait(0.01);
59010050 2:436ed0069b61 87 pc.printf("stage3");
59010050 2:436ed0069b61 88 if(pos_down == 1395 and pos_up == edit_up+5){
59010050 2:436ed0069b61 89 state_count2 = 4;
59010050 2:436ed0069b61 90 }
59010050 2:436ed0069b61 91 }
59010050 2:436ed0069b61 92 else if(state_count2 == 4){
59010050 2:436ed0069b61 93 Servo2.SetPosition(pos_up);
59010050 2:436ed0069b61 94 //pos_up = pos_up-5;
59010050 2:436ed0069b61 95 wait(0.01);
59010050 2:436ed0069b61 96 pc.printf("stage4");
59010050 2:436ed0069b61 97 if(pos_down == 1395 and pos_up == 995){
59010050 2:436ed0069b61 98 state_count2 = 0;
59010050 2:436ed0069b61 99 }
59010050 2:436ed0069b61 100 }
59010050 2:436ed0069b61 101 else if (state_count2 == 0 and round_count2 < 10){
59010050 2:436ed0069b61 102 round_count2 = round_count2+1;
59010050 2:436ed0069b61 103 state_count2 = 1;
59010050 2:436ed0069b61 104 pos_down = 1400;
59010050 2:436ed0069b61 105 pos_up = 1000;
59010050 2:436ed0069b61 106 }
59010050 2:436ed0069b61 107 else {
59010050 2:436ed0069b61 108 pc.printf("1stop");
59010050 2:436ed0069b61 109 //thread1.stop(IMU);
59010050 2:436ed0069b61 110 }
59010050 2:436ed0069b61 111
59010050 2:436ed0069b61 112
59010050 2:436ed0069b61 113 }
59010050 1:852156b5cca1 114 void move() {
59010050 1:852156b5cca1 115 attitude_setup();
Khanchana 0:3af6fae90816 116 Servo1.Enable(1000,2000);
Khanchana 0:3af6fae90816 117 Servo2.Enable(1000,2000);
Khanchana 0:3af6fae90816 118 Servo3.Enable(1000,2000);
Khanchana 0:3af6fae90816 119 Servo4.Enable(1000,2000);
59010050 2:436ed0069b61 120 //pc.printf("start");
59010050 2:436ed0069b61 121 //pc.printf("start");
59010050 2:436ed0069b61 122 //pc.printf("start");
59010050 2:436ed0069b61 123 //pc.printf("start");
59010050 1:852156b5cca1 124 pc.printf("start");
Khanchana 0:3af6fae90816 125 while(1){
59010050 2:436ed0069b61 126 servoleft();
59010050 1:852156b5cca1 127 if(state_count == 1){
59010050 1:852156b5cca1 128 Servo3.SetPosition(pos_down);
59010050 1:852156b5cca1 129 pos_down = pos_down+5;
59010050 1:852156b5cca1 130 wait(0.01);
59010050 2:436ed0069b61 131 pc.printf("right %d\n",pos_down);
59010050 2:436ed0069b61 132 if(pos_down == pos_down_end+5 and pos_up == 1000){
59010050 1:852156b5cca1 133 state_count = 2;
59010050 1:852156b5cca1 134 }
Khanchana 0:3af6fae90816 135 }
59010050 1:852156b5cca1 136 else if(state_count == 2){
59010050 1:852156b5cca1 137 Servo4.SetPosition(pos_up);
59010050 1:852156b5cca1 138 pos_up = pos_up+5;
59010050 1:852156b5cca1 139 wait(0.01);
59010050 2:436ed0069b61 140 if(pos_down == pos_down_end+5 and pos_up == pos_up_end+5){
59010050 1:852156b5cca1 141 state_count = 3;
59010050 1:852156b5cca1 142 }
Khanchana 0:3af6fae90816 143 }
59010050 1:852156b5cca1 144 else if(state_count == 3){
59010050 1:852156b5cca1 145 Servo3.SetPosition(pos_down);
59010050 1:852156b5cca1 146 pos_down = pos_down-5;
59010050 1:852156b5cca1 147 wait(0.01);
59010050 2:436ed0069b61 148 if(pos_down == 1395 and pos_up == pos_up_end+5){
59010050 1:852156b5cca1 149 state_count = 4;
59010050 1:852156b5cca1 150 }
Khanchana 0:3af6fae90816 151 }
59010050 1:852156b5cca1 152 else if(state_count == 4){
59010050 1:852156b5cca1 153 Servo4.SetPosition(pos_up);
59010050 1:852156b5cca1 154 pos_up = pos_up-5;
59010050 1:852156b5cca1 155 wait(0.01);
59010050 2:436ed0069b61 156 if(pos_down == 1395 and pos_up == 995){
59010050 1:852156b5cca1 157 state_count = 0;
59010050 1:852156b5cca1 158 }
59010050 1:852156b5cca1 159 }
59010050 1:852156b5cca1 160 else if (state_count == 0 and round_count < 10){
59010050 1:852156b5cca1 161 round_count = round_count+1;
59010050 1:852156b5cca1 162 state_count = 1;
59010050 1:852156b5cca1 163 pos_down = 1400;
59010050 1:852156b5cca1 164 pos_up = 1000;
59010050 1:852156b5cca1 165 }
59010050 1:852156b5cca1 166 else {
59010050 1:852156b5cca1 167 pc.printf("stop");
59010050 1:852156b5cca1 168 }
Khanchana 0:3af6fae90816 169 }
59010050 1:852156b5cca1 170 }