TurtleBot / Mbed 2 deprecated TurtleBot_V5-2

latest

Dependencies:   Servo mbed-rtos mbed

Fork of TurtleBot_V555 by Sopitchaya Lummaetee

main.cpp@3:98ef5105926e, 2018-04-04 (annotated)

Committer:
59010050
Date:
Wed Apr 04 14:47:57 2018 +0000
Revision:
3:98ef5105926e
Parent:
2:436ed0069b61
Child:
4:6c8b844d291f
last update

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