TurtleBot / Mbed 2 deprecated TurtleBot_V5-2

latest

Dependencies:   Servo mbed-rtos mbed

Fork of TurtleBot_V555 by Sopitchaya Lummaetee

main.cpp@5:d5c2e8f852fd, 2018-04-06 (annotated)

Committer:
59010050
Date:
Fri Apr 06 08:33:48 2018 +0000
Revision:
5:d5c2e8f852fd
Parent:
4:6c8b844d291f 
latest

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 5:d5c2e8f852fd 9 Timer timerwalk;
59010050 1:852156b5cca1 10 Thread thread1;
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 3:98ef5105926e 19 //pc.printf("IMU \n");
59010050 1:852156b5cca1 20 pc.printf(" %f \t", ax*10 );
59010050 1:852156b5cca1 21 pc.printf(" %f \t", ay*10 );
59010050 3:98ef5105926e 22 pc.printf(" %f \t", az*10 -10); //cm/s*s
59010050 1:852156b5cca1 23
59010050 1:852156b5cca1 24 pc.printf(" %f \t", gx );
59010050 1:852156b5cca1 25 pc.printf(" %f \t", gy );
59010050 1:852156b5cca1 26 pc.printf(" %f \t", gz ); //deg/s */
59010050 3:98ef5105926e 27 pc.printf("%.0f\t %.0f \t %.0f \n\r", roll, pitch, yaw);
59010050 1:852156b5cca1 28
59010050 1:852156b5cca1 29 timer1.reset(); // reset timer
59010050 1:852156b5cca1 30 }
59010050 1:852156b5cca1 31 }
59010050 1:852156b5cca1 32 }
59010050 1:852156b5cca1 33
Khanchana 0:3af6fae90816 34 Servo Servo1(D6);
Khanchana 0:3af6fae90816 35 Servo Servo2(D8);
Khanchana 0:3af6fae90816 36 Servo Servo3(D9);
Khanchana 0:3af6fae90816 37 Servo Servo4(D10);
Khanchana 0:3af6fae90816 38
59010050 3:98ef5105926e 39 int value;
59010050 5:d5c2e8f852fd 40 int walking_time;
Khanchana 0:3af6fae90816 41
59010050 3:98ef5105926e 42 void servo_Right();
59010050 3:98ef5105926e 43 void move();
59010050 3:98ef5105926e 44 void cal_step_down();
59010050 3:98ef5105926e 45 void cal_step_up();
59010050 3:98ef5105926e 46 void servo();
59010050 3:98ef5105926e 47 void getvalue();
59010050 3:98ef5105926e 48
59010050 3:98ef5105926e 49 float pos_down_start = 1400.00;
59010050 3:98ef5105926e 50 float pos_up_start = 1000.00;
59010050 4:6c8b844d291f 51 float down_degree = 0.00 ;
59010050 4:6c8b844d291f 52 float up_degree = 0.00 ;
59010050 3:98ef5105926e 53 float stepmin = 1;
59010050 5:d5c2e8f852fd 54 float round = 5;
59010050 3:98ef5105926e 55 float waittime = 0.001 ;
59010050 3:98ef5105926e 56
59010050 3:98ef5105926e 57 float pos_down_left = 1400.00;
59010050 3:98ef5105926e 58 float pos_up_left = 1000.00;
59010050 3:98ef5105926e 59 float pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); //left down //90 ใน80 นอก(45)+7 ใน85 นอก+5
59010050 3:98ef5105926e 60 float pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); //left up// 15 , 30+10 , 45-1.75 , 60-5 , 75-5 , 45+5
59010050 3:98ef5105926e 61 float state_count_left = 1;
59010050 3:98ef5105926e 62 float round_count_left = 1;
59010050 3:98ef5105926e 63 float step_down_left;
59010050 3:98ef5105926e 64 float step_up_left;
59010050 3:98ef5105926e 65
59010050 3:98ef5105926e 66 float pos_down_right = 1400.00;
59010050 3:98ef5105926e 67 float pos_up_right = 1000.00;
59010050 3:98ef5105926e 68 float pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree))) ; //right down //99
59010050 3:98ef5105926e 69 float pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); //right up// 15 , 30-10 , 45+1.75 , 60+5 , 75+5 , 45-5
59010050 3:98ef5105926e 70 float state_count_right = 1;
59010050 3:98ef5105926e 71 float round_count_right = 1;
59010050 3:98ef5105926e 72 float step_up_right;
59010050 3:98ef5105926e 73 float step_down_right;
59010050 3:98ef5105926e 74
59010050 3:98ef5105926e 75
59010050 3:98ef5105926e 76
Khanchana 0:3af6fae90816 77 int main() {
59010050 1:852156b5cca1 78 pc.baud(1000000);
59010050 2:436ed0069b61 79 //pc.printf("malin");
59010050 1:852156b5cca1 80 timer1.start(); // start timer counting
59010050 4:6c8b844d291f 81 //if (pc.getc() == '1')
59010050 5:d5c2e8f852fd 82 //{
59010050 3:98ef5105926e 83 thread2.start(servo);
59010050 4:6c8b844d291f 84 //thread1.start(IMU);
59010050 5:d5c2e8f852fd 85 //}
59010050 1:852156b5cca1 86 }
59010050 3:98ef5105926e 87 void cal_step_down(){
59010050 4:6c8b844d291f 88 pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree)));
59010050 4:6c8b844d291f 89 pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree))) ;
59010050 3:98ef5105926e 90 if (pos_down_end_right > pos_down_end_left){
59010050 3:98ef5105926e 91 step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start);
59010050 3:98ef5105926e 92 step_down_left = stepmin;
59010050 3:98ef5105926e 93 } else if (pos_down_end_right < pos_down_end_left){
59010050 3:98ef5105926e 94 step_down_right = stepmin;
59010050 3:98ef5105926e 95 step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
59010050 3:98ef5105926e 96 } else{
59010050 3:98ef5105926e 97 step_down_right = stepmin;
59010050 3:98ef5105926e 98 step_down_left = stepmin;
59010050 2:436ed0069b61 99 }
59010050 5:d5c2e8f852fd 100 /*pc.printf("pos_down_right");
59010050 3:98ef5105926e 101 pc.printf("%f\n",pos_down_end_right);
59010050 3:98ef5105926e 102 pc.printf("pos_down_left");
59010050 3:98ef5105926e 103 pc.printf("%f\n",pos_down_end_left);
59010050 3:98ef5105926e 104 pc.printf("step_down_right");
59010050 3:98ef5105926e 105 pc.printf("%f\n",step_down_right);
59010050 3:98ef5105926e 106 pc.printf("step_down_left");
59010050 5:d5c2e8f852fd 107 pc.printf("%f\n",step_down_left); */
59010050 3:98ef5105926e 108 }
59010050 3:98ef5105926e 109
59010050 3:98ef5105926e 110 void cal_step_up(){
59010050 4:6c8b844d291f 111 pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree));
59010050 4:6c8b844d291f 112 pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree));
59010050 3:98ef5105926e 113 if (pos_up_end_right > pos_up_end_left){
59010050 3:98ef5105926e 114 step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start);
59010050 3:98ef5105926e 115 step_up_left = stepmin;
59010050 3:98ef5105926e 116 } else if (pos_up_end_right < pos_up_end_left){
59010050 3:98ef5105926e 117 step_up_right = stepmin;
59010050 3:98ef5105926e 118 step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
59010050 3:98ef5105926e 119 } else{
59010050 3:98ef5105926e 120 step_up_right = stepmin;
59010050 3:98ef5105926e 121 step_up_left = stepmin;
59010050 3:98ef5105926e 122 }
59010050 5:d5c2e8f852fd 123 /*pc.printf("pos_up_right");
59010050 3:98ef5105926e 124 pc.printf("%f\n",pos_up_end_right);
59010050 3:98ef5105926e 125 pc.printf("pos_up_left");
59010050 3:98ef5105926e 126 pc.printf("%f\n",pos_up_end_left);
59010050 3:98ef5105926e 127 pc.printf("step_up_right");
59010050 3:98ef5105926e 128 pc.printf("%f\n",step_up_right);;
59010050 3:98ef5105926e 129 pc.printf("step_up_left");
59010050 5:d5c2e8f852fd 130 pc.printf("%f\n",step_up_left); */
59010050 3:98ef5105926e 131 }
59010050 3:98ef5105926e 132
59010050 3:98ef5105926e 133 void move(){
Khanchana 0:3af6fae90816 134 Servo1.Enable(1000,2000);
Khanchana 0:3af6fae90816 135 Servo2.Enable(1000,2000);
Khanchana 0:3af6fae90816 136 Servo3.Enable(1000,2000);
Khanchana 0:3af6fae90816 137 Servo4.Enable(1000,2000);
59010050 3:98ef5105926e 138 while(1) {
59010050 3:98ef5105926e 139 servo_Right();
59010050 3:98ef5105926e 140 if(state_count_left == 1) {
59010050 3:98ef5105926e 141 Servo1.SetPosition(pos_down_left);
59010050 3:98ef5105926e 142 wait(waittime);
59010050 3:98ef5105926e 143 pos_down_left = pos_down_left + step_down_left;
59010050 3:98ef5105926e 144 if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) {
59010050 3:98ef5105926e 145 state_count_left = 2;
59010050 3:98ef5105926e 146 }
59010050 3:98ef5105926e 147 /*pc.printf("LAD");
59010050 3:98ef5105926e 148 pc.printf("%f\n",pos_down_left);
59010050 3:98ef5105926e 149 pc.printf("LAP");
59010050 3:98ef5105926e 150 pc.printf("%f\n",pos_up_left);*/
59010050 3:98ef5105926e 151 } else if(state_count_left == 2) {
59010050 3:98ef5105926e 152 Servo2.SetPosition(pos_up_left);
59010050 3:98ef5105926e 153 wait(waittime);
59010050 3:98ef5105926e 154 pos_up_left = pos_up_left + step_up_left;
59010050 3:98ef5105926e 155 if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
59010050 3:98ef5105926e 156 state_count_left = 3;
59010050 3:98ef5105926e 157 }
59010050 3:98ef5105926e 158 /*pc.printf("LBD");
59010050 3:98ef5105926e 159 pc.printf("%f\n",pos_down_left);
59010050 3:98ef5105926e 160 pc.printf("LBP");
59010050 3:98ef5105926e 161 pc.printf("%f\n",pos_up_left);*/
59010050 3:98ef5105926e 162 } else if(state_count_left == 3) {
59010050 3:98ef5105926e 163 Servo1.SetPosition(pos_down_left);
59010050 3:98ef5105926e 164 wait(waittime);
59010050 3:98ef5105926e 165 pos_down_left = pos_down_left - step_down_left;
59010050 3:98ef5105926e 166 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
59010050 3:98ef5105926e 167 state_count_left = 4;
59010050 3:98ef5105926e 168 }
59010050 3:98ef5105926e 169 /*pc.printf("LCD");
59010050 3:98ef5105926e 170 pc.printf("%f\n",pos_down_left);
59010050 3:98ef5105926e 171 pc.printf("LCP");
59010050 3:98ef5105926e 172 pc.printf("%f\n",pos_up_left);*/
59010050 3:98ef5105926e 173 } else if(state_count_left == 4) {
59010050 3:98ef5105926e 174 Servo2.SetPosition(pos_up_left);
59010050 3:98ef5105926e 175 wait(waittime);
59010050 3:98ef5105926e 176 pos_up_left = pos_up_left - step_up_left;
59010050 3:98ef5105926e 177 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) {
59010050 3:98ef5105926e 178 state_count_left = 0;
59010050 1:852156b5cca1 179 }
59010050 3:98ef5105926e 180 /*pc.printf("LDD");
59010050 3:98ef5105926e 181 pc.printf("%f\n",pos_down_left);
59010050 3:98ef5105926e 182 pc.printf("LDP");
59010050 3:98ef5105926e 183 pc.printf("%f\n",pos_up_left);*/
59010050 3:98ef5105926e 184 } else if (state_count_left == 0 and round_count_left < round) {
59010050 3:98ef5105926e 185 round_count_left = round_count_left+1;
59010050 3:98ef5105926e 186 state_count_left = 1;
59010050 3:98ef5105926e 187 pos_down_left = pos_down_start;
59010050 3:98ef5105926e 188 pos_up_left = pos_up_start;
59010050 3:98ef5105926e 189 } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round){
59010050 5:d5c2e8f852fd 190 pc.printf("Finish \n");
59010050 5:d5c2e8f852fd 191 walking_time = timerwalk.read_ms();
59010050 3:98ef5105926e 192 thread1.terminate();
59010050 5:d5c2e8f852fd 193 pc.printf("Walking time = %d \n", walking_time);
59010050 3:98ef5105926e 194 break;
Khanchana 0:3af6fae90816 195 }
59010050 3:98ef5105926e 196 }
59010050 3:98ef5105926e 197 }
59010050 3:98ef5105926e 198
59010050 3:98ef5105926e 199 void servo_Right()
59010050 3:98ef5105926e 200 {
59010050 3:98ef5105926e 201 if(state_count_right == 1) {
59010050 3:98ef5105926e 202 Servo3.SetPosition(pos_down_right);
59010050 3:98ef5105926e 203 wait(waittime);
59010050 3:98ef5105926e 204 pos_down_right = pos_down_right + step_down_right;
59010050 3:98ef5105926e 205 if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) {
59010050 3:98ef5105926e 206 state_count_right = 2;
59010050 3:98ef5105926e 207 }
59010050 3:98ef5105926e 208 /*pc.printf("RAD");
59010050 3:98ef5105926e 209 pc.printf("%f\n",pos_down_right);
59010050 3:98ef5105926e 210 pc.printf("RAP");
59010050 3:98ef5105926e 211 pc.printf("%f\n",pos_up_right);*/
59010050 3:98ef5105926e 212 } else if(state_count_right == 2) {
59010050 3:98ef5105926e 213 Servo4.SetPosition(pos_up_right);
59010050 3:98ef5105926e 214 wait(waittime);
59010050 3:98ef5105926e 215 pos_up_right = pos_up_right + step_up_right;
59010050 3:98ef5105926e 216 if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
59010050 3:98ef5105926e 217 state_count_right = 3;
59010050 3:98ef5105926e 218 }
59010050 3:98ef5105926e 219 /*pc.printf("RBD");
59010050 3:98ef5105926e 220 pc.printf("%f\n",pos_down_right);
59010050 3:98ef5105926e 221 pc.printf("RBP");
59010050 3:98ef5105926e 222 pc.printf("%f\n",pos_up_right);*/
59010050 3:98ef5105926e 223 } else if(state_count_right == 3) {
59010050 3:98ef5105926e 224 Servo3.SetPosition(pos_down_right);
59010050 3:98ef5105926e 225 wait(waittime);
59010050 3:98ef5105926e 226 pos_down_right = pos_down_right - step_down_right;
59010050 3:98ef5105926e 227 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
59010050 3:98ef5105926e 228 state_count_right = 4;
Khanchana 0:3af6fae90816 229 }
59010050 3:98ef5105926e 230 /*pc.printf("RCD");
59010050 3:98ef5105926e 231 pc.printf("%f\n",pos_down_right);
59010050 3:98ef5105926e 232 pc.printf("RCP");
59010050 3:98ef5105926e 233 pc.printf("%f\n",pos_up_right);*/
59010050 3:98ef5105926e 234 } else if(state_count_right == 4) {
59010050 3:98ef5105926e 235 Servo4.SetPosition(pos_up_right);
59010050 3:98ef5105926e 236 wait(waittime);
59010050 3:98ef5105926e 237 pos_up_right = pos_up_right - step_up_right;
59010050 3:98ef5105926e 238 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) {
59010050 3:98ef5105926e 239 state_count_right = 0;
Khanchana 0:3af6fae90816 240 }
59010050 3:98ef5105926e 241 /*pc.printf("RDD");
59010050 3:98ef5105926e 242 pc.printf("%f\n",pos_down_right);
59010050 3:98ef5105926e 243 pc.printf("RDP");
59010050 3:98ef5105926e 244 pc.printf("%f\n",pos_up_right);*/
59010050 3:98ef5105926e 245 } else if (state_count_right == 0 and round_count_right < round) {
59010050 3:98ef5105926e 246 round_count_right = round_count_right+1;
59010050 3:98ef5105926e 247 state_count_right = 1;
59010050 3:98ef5105926e 248 pos_down_right = pos_down_start;
59010050 3:98ef5105926e 249 pos_up_right = pos_up_start;
Khanchana 0:3af6fae90816 250 }
59010050 3:98ef5105926e 251 }
59010050 3:98ef5105926e 252
59010050 3:98ef5105926e 253
59010050 3:98ef5105926e 254 void servo() {
59010050 3:98ef5105926e 255 attitude_setup();
59010050 4:6c8b844d291f 256 getvalue();
59010050 3:98ef5105926e 257 pc.printf("start\n");
59010050 5:d5c2e8f852fd 258 thread1.start(IMU);
59010050 5:d5c2e8f852fd 259 timerwalk.start(); // start timer counting
59010050 4:6c8b844d291f 260 //pc.printf("%f \n",down_degree );
59010050 4:6c8b844d291f 261 //pc.printf("%f \n",up_degree );
59010050 3:98ef5105926e 262 cal_step_down();
59010050 3:98ef5105926e 263 cal_step_up();
59010050 4:6c8b844d291f 264 //pc.printf("%f \n",down_degree );
59010050 4:6c8b844d291f 265 //pc.printf("%f \n",up_degree );
59010050 3:98ef5105926e 266 move();
59010050 3:98ef5105926e 267 }
59010050 3:98ef5105926e 268
59010050 3:98ef5105926e 269 void getvalue() {
59010050 3:98ef5105926e 270 pc.printf("case 1 = 90-15 \n");
59010050 3:98ef5105926e 271 pc.printf("case 2 = 90-30 \n");
59010050 3:98ef5105926e 272 pc.printf("case 3 = 90-45 \n");
59010050 3:98ef5105926e 273 pc.printf("case 4 = 90-60 \n");
59010050 5:d5c2e8f852fd 274 pc.printf("case 5 = 90-75 \n");
59010050 5:d5c2e8f852fd 275 pc.printf("case 6 = 80-45 \n");
59010050 5:d5c2e8f852fd 276 pc.printf("case 7 = 85-45 \n");
59010050 5:d5c2e8f852fd 277 pc.printf("case 8 = 95-45 \n");
59010050 5:d5c2e8f852fd 278 pc.printf("case 9 = 100-45 \n");
59010050 3:98ef5105926e 279 value = pc.getc() ;
59010050 3:98ef5105926e 280 switch (value) {
59010050 3:98ef5105926e 281 case '1': {
59010050 3:98ef5105926e 282 down_degree = 90.00 ;
59010050 3:98ef5105926e 283 up_degree = 15.00 ;
59010050 3:98ef5105926e 284 break;
59010050 3:98ef5105926e 285 }
59010050 3:98ef5105926e 286 case '2': {
59010050 3:98ef5105926e 287 down_degree = 90.00 ;
59010050 3:98ef5105926e 288 up_degree = 30.00 ;
59010050 3:98ef5105926e 289 break;
59010050 3:98ef5105926e 290 }
59010050 3:98ef5105926e 291 case '3': {
59010050 3:98ef5105926e 292 down_degree = 90.00 ;
59010050 3:98ef5105926e 293 up_degree = 45.00 ;
59010050 3:98ef5105926e 294 break;
59010050 3:98ef5105926e 295 }
59010050 3:98ef5105926e 296 case '4': {
59010050 3:98ef5105926e 297 down_degree = 90.00 ;
59010050 3:98ef5105926e 298 up_degree = 60.00 ;
59010050 3:98ef5105926e 299 break;
59010050 3:98ef5105926e 300 }
59010050 3:98ef5105926e 301 case '5': {
59010050 5:d5c2e8f852fd 302 down_degree = 90.00 ;
59010050 5:d5c2e8f852fd 303 up_degree = 75.00 ;
59010050 5:d5c2e8f852fd 304 break;
59010050 5:d5c2e8f852fd 305 }
59010050 5:d5c2e8f852fd 306 case '6': {
59010050 3:98ef5105926e 307 down_degree = 80.00 ;
59010050 3:98ef5105926e 308 up_degree = 45.00 ;
59010050 3:98ef5105926e 309 break;
59010050 3:98ef5105926e 310 }
59010050 5:d5c2e8f852fd 311 case '7': {
59010050 3:98ef5105926e 312 down_degree = 85.00 ;
59010050 3:98ef5105926e 313 up_degree = 45.00 ;
59010050 3:98ef5105926e 314 break;
59010050 3:98ef5105926e 315 }
59010050 5:d5c2e8f852fd 316 case '8': {
59010050 3:98ef5105926e 317 down_degree = 95.00 ;
59010050 3:98ef5105926e 318 up_degree = 45.00 ;
59010050 3:98ef5105926e 319 break;
59010050 3:98ef5105926e 320 }
59010050 5:d5c2e8f852fd 321 case '9': {
59010050 3:98ef5105926e 322 down_degree = 100.00 ;
59010050 3:98ef5105926e 323 up_degree = 45.00 ;
59010050 3:98ef5105926e 324 break;
59010050 3:98ef5105926e 325 }
59010050 3:98ef5105926e 326 break;
59010050 3:98ef5105926e 327 }
59010050 4:6c8b844d291f 328 //pc.printf("%f \n",down_degree );
59010050 4:6c8b844d291f 329 //pc.printf("%f \n",up_degree );
59010050 3:98ef5105926e 330
59010050 3:98ef5105926e 331 }