TurtleBot / Mbed 2 deprecated testLibrary

malin

Dependencies:   Servo mbed mbed-rtos

main.cpp@4:d00236029c9d, 2018-04-02 (annotated)

Committer:
Khanchana
Date:
Mon Apr 02 16:41:17 2018 +0000
Revision:
4:d00236029c9d
Parent:
3:64bfd330beb4 
SERVOOOO

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Khanchana 0:596de9a5c573 1 #include "mbed.h"
Khanchana 0:596de9a5c573 2 #include "Servo.h"
Khanchana 1:da79761252bc 3 #include "rtos.h"
Khanchana 2:68dbcd5277e4 4
Khanchana 2:68dbcd5277e4 5 Serial pc(USBTX, USBRX);
Khanchana 3:64bfd330beb4 6
Khanchana 3:64bfd330beb4 7 Thread thread;
Khanchana 4:d00236029c9d 8
Khanchana 2:68dbcd5277e4 9 Servo Servo1(D6);
Khanchana 2:68dbcd5277e4 10 Servo Servo2(D8);
Khanchana 1:da79761252bc 11 Servo Servo3(D9);
Khanchana 2:68dbcd5277e4 12 Servo Servo4(D10);
Khanchana 2:68dbcd5277e4 13
Khanchana 3:64bfd330beb4 14 void servo_Right();
Khanchana 3:64bfd330beb4 15 void move();
Khanchana 4:d00236029c9d 16 void cal_step_down();
Khanchana 4:d00236029c9d 17 void cal_step_up();
Khanchana 1:da79761252bc 18
Khanchana 3:64bfd330beb4 19 float pos_down_start = 1400;
Khanchana 3:64bfd330beb4 20 float pos_up_start = 1000;
Khanchana 4:d00236029c9d 21 float stepmin = 5;
Khanchana 4:d00236029c9d 22 float round = 5;
Khanchana 4:d00236029c9d 23
Khanchana 4:d00236029c9d 24 float pos_down_left = 1400;
Khanchana 4:d00236029c9d 25 float pos_up_left = 1000;
Khanchana 4:d00236029c9d 26 float pos_down_end_left = 1700; //left down
Khanchana 4:d00236029c9d 27 float pos_up_end_left = 1350; //left up
Khanchana 4:d00236029c9d 28 float state_count_left = 1;
Khanchana 4:d00236029c9d 29 float round_count_left = 1;
Khanchana 4:d00236029c9d 30 float step_down_left;
Khanchana 4:d00236029c9d 31 float step_up_left;
Khanchana 4:d00236029c9d 32
Khanchana 3:64bfd330beb4 33 float pos_down_right = 1400;
Khanchana 3:64bfd330beb4 34 float pos_up_right = 1000;
Khanchana 4:d00236029c9d 35 float pos_down_end_right = 1650; //right down
Khanchana 4:d00236029c9d 36 float pos_up_end_right = 1700; //right up
Khanchana 3:64bfd330beb4 37 float state_count_right = 1;
Khanchana 3:64bfd330beb4 38 float round_count_right = 1;
Khanchana 4:d00236029c9d 39 float step_up_right;
Khanchana 4:d00236029c9d 40 float step_down_right;
Khanchana 2:68dbcd5277e4 41
Khanchana 4:d00236029c9d 42 int main()
Khanchana 4:d00236029c9d 43 {
Khanchana 4:d00236029c9d 44 wait(5);
Khanchana 4:d00236029c9d 45 cal_step_down();
Khanchana 4:d00236029c9d 46 cal_step_up();
Khanchana 4:d00236029c9d 47 move();
Khanchana 3:64bfd330beb4 48 }
Khanchana 3:64bfd330beb4 49
Khanchana 4:d00236029c9d 50 void cal_step_down(){
Khanchana 4:d00236029c9d 51 if (pos_down_end_right > pos_down_end_left){
Khanchana 4:d00236029c9d 52 step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start);
Khanchana 4:d00236029c9d 53 step_down_left = stepmin;
Khanchana 4:d00236029c9d 54 } else if (pos_down_end_right < pos_down_end_left){
Khanchana 4:d00236029c9d 55 step_down_right = stepmin;
Khanchana 4:d00236029c9d 56 step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
Khanchana 4:d00236029c9d 57 } else{
Khanchana 4:d00236029c9d 58 step_down_right = stepmin;
Khanchana 4:d00236029c9d 59 step_down_left = stepmin;
Khanchana 4:d00236029c9d 60 }
Khanchana 4:d00236029c9d 61 /*pc.printf("pos_down_right");
Khanchana 4:d00236029c9d 62 pc.printf("%f\n",pos_down_end_right);
Khanchana 4:d00236029c9d 63 pc.printf("pos_down_left");
Khanchana 4:d00236029c9d 64 pc.printf("%f\n",pos_down_end_left);
Khanchana 4:d00236029c9d 65 pc.printf("step_down_right");
Khanchana 4:d00236029c9d 66 pc.printf("%f\n",step_down_right);
Khanchana 4:d00236029c9d 67 pc.printf("step_down_left");
Khanchana 4:d00236029c9d 68 pc.printf("%f\n",step_down_left);*/
Khanchana 4:d00236029c9d 69 }
Khanchana 4:d00236029c9d 70
Khanchana 4:d00236029c9d 71 void cal_step_up(){
Khanchana 4:d00236029c9d 72 if (pos_up_end_right > pos_up_end_left){
Khanchana 4:d00236029c9d 73 step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start);
Khanchana 4:d00236029c9d 74 step_up_left = stepmin;
Khanchana 4:d00236029c9d 75 } else if (pos_up_end_right < pos_up_end_left){
Khanchana 4:d00236029c9d 76 step_up_right = stepmin;
Khanchana 4:d00236029c9d 77 step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
Khanchana 4:d00236029c9d 78 } else{
Khanchana 4:d00236029c9d 79 step_up_right = stepmin;
Khanchana 4:d00236029c9d 80 step_up_left = stepmin;
Khanchana 4:d00236029c9d 81 }
Khanchana 4:d00236029c9d 82 /*pc.printf("pos_up_right");
Khanchana 4:d00236029c9d 83 pc.printf("%f\n",pos_up_end_right);
Khanchana 4:d00236029c9d 84 pc.printf("pos_up_left");
Khanchana 4:d00236029c9d 85 pc.printf("%f\n",pos_up_end_left);
Khanchana 4:d00236029c9d 86 pc.printf("step_up_right");
Khanchana 4:d00236029c9d 87 pc.printf("%f\n",step_up_right);;
Khanchana 4:d00236029c9d 88 pc.printf("step_up_left");
Khanchana 4:d00236029c9d 89 pc.printf("%f\n",step_up_left);*/
Khanchana 4:d00236029c9d 90 }
Khanchana 4:d00236029c9d 91
Khanchana 4:d00236029c9d 92 void move(){
Khanchana 2:68dbcd5277e4 93 pc.baud(9600);
Khanchana 2:68dbcd5277e4 94 Servo1.Enable(1000,2000);
Khanchana 2:68dbcd5277e4 95 Servo2.Enable(1000,2000);
Khanchana 2:68dbcd5277e4 96 Servo3.Enable(1000,2000);
Khanchana 2:68dbcd5277e4 97 Servo4.Enable(1000,2000);
Khanchana 4:d00236029c9d 98 pc.printf("Start\n");
Khanchana 4:d00236029c9d 99 while(1) {
Khanchana 3:64bfd330beb4 100 servo_Right();
Khanchana 4:d00236029c9d 101 if(state_count_left == 1) {
Khanchana 4:d00236029c9d 102 Servo1.SetPosition(pos_down_left);
Khanchana 4:d00236029c9d 103 wait(0.005);
Khanchana 4:d00236029c9d 104 pos_down_left = pos_down_left + step_down_left;
Khanchana 4:d00236029c9d 105 if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) {
Khanchana 4:d00236029c9d 106 state_count_left = 2;
Khanchana 3:64bfd330beb4 107 }
Khanchana 4:d00236029c9d 108 /*pc.printf("LAD");
Khanchana 4:d00236029c9d 109 pc.printf("%f\n",pos_down_left);
Khanchana 3:64bfd330beb4 110 pc.printf("LAP");
Khanchana 4:d00236029c9d 111 pc.printf("%f\n",pos_up_left);*/
Khanchana 4:d00236029c9d 112 } else if(state_count_left == 2) {
Khanchana 4:d00236029c9d 113 Servo2.SetPosition(pos_up_left);
Khanchana 4:d00236029c9d 114 wait(0.005);
Khanchana 4:d00236029c9d 115 pos_up_left = pos_up_left + step_up_left;
Khanchana 4:d00236029c9d 116 if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
Khanchana 4:d00236029c9d 117 state_count_left = 3;
Khanchana 4:d00236029c9d 118 }
Khanchana 4:d00236029c9d 119 /*pc.printf("LBD");
Khanchana 4:d00236029c9d 120 pc.printf("%f\n",pos_down_left);
Khanchana 4:d00236029c9d 121 pc.printf("LBP");
Khanchana 4:d00236029c9d 122 pc.printf("%f\n",pos_up_left);*/
Khanchana 4:d00236029c9d 123 } else if(state_count_left == 3) {
Khanchana 4:d00236029c9d 124 Servo1.SetPosition(pos_down_left);
Khanchana 4:d00236029c9d 125 wait(0.005);
Khanchana 4:d00236029c9d 126 pos_down_left = pos_down_left - step_down_left;
Khanchana 4:d00236029c9d 127 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
Khanchana 4:d00236029c9d 128 state_count_left = 4;
Khanchana 3:64bfd330beb4 129 }
Khanchana 4:d00236029c9d 130 /*pc.printf("LCD");
Khanchana 4:d00236029c9d 131 pc.printf("%f\n",pos_down_left);
Khanchana 4:d00236029c9d 132 pc.printf("LCP");
Khanchana 4:d00236029c9d 133 pc.printf("%f\n",pos_up_left);*/
Khanchana 4:d00236029c9d 134 } else if(state_count_left == 4) {
Khanchana 4:d00236029c9d 135 Servo2.SetPosition(pos_up_left);
Khanchana 4:d00236029c9d 136 wait(0.005);
Khanchana 4:d00236029c9d 137 pos_up_left = pos_up_left - step_up_left;
Khanchana 4:d00236029c9d 138 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) {
Khanchana 4:d00236029c9d 139 state_count_left = 0;
Khanchana 4:d00236029c9d 140 }
Khanchana 4:d00236029c9d 141 /*pc.printf("LDD");
Khanchana 4:d00236029c9d 142 pc.printf("%f\n",pos_down_left);
Khanchana 4:d00236029c9d 143 pc.printf("LDP");
Khanchana 4:d00236029c9d 144 pc.printf("%f\n",pos_up_left);*/
Khanchana 4:d00236029c9d 145 } else if (state_count_left == 0 and round_count_left < round) {
Khanchana 4:d00236029c9d 146 round_count_left = round_count_left+1;
Khanchana 4:d00236029c9d 147 state_count_left = 1;
Khanchana 4:d00236029c9d 148 pos_down_left = pos_down_start;
Khanchana 4:d00236029c9d 149 pos_up_left = pos_up_start;
Khanchana 4:d00236029c9d 150 } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right < round){
Khanchana 4:d00236029c9d 151 pc.printf("Finish");
Khanchana 4:d00236029c9d 152 break;
Khanchana 1:da79761252bc 153 }
Khanchana 1:da79761252bc 154 }
Khanchana 1:da79761252bc 155 }
Khanchana 2:68dbcd5277e4 156
Khanchana 4:d00236029c9d 157 void servo_Right()
Khanchana 4:d00236029c9d 158 {
Khanchana 4:d00236029c9d 159 if(state_count_right == 1) {
Khanchana 4:d00236029c9d 160 Servo3.SetPosition(pos_down_right);
Khanchana 4:d00236029c9d 161 wait(0.005);
Khanchana 4:d00236029c9d 162 pos_down_right = pos_down_right + step_down_right;
Khanchana 4:d00236029c9d 163 if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) {
Khanchana 4:d00236029c9d 164 state_count_right = 2;
Khanchana 0:596de9a5c573 165 }
Khanchana 4:d00236029c9d 166 /*pc.printf("RAD");
Khanchana 4:d00236029c9d 167 pc.printf("%f\n",pos_down_right);
Khanchana 4:d00236029c9d 168 pc.printf("RAP");
Khanchana 4:d00236029c9d 169 pc.printf("%f\n",pos_up_right);*/
Khanchana 4:d00236029c9d 170 } else if(state_count_right == 2) {
Khanchana 4:d00236029c9d 171 Servo4.SetPosition(pos_up_right);
Khanchana 4:d00236029c9d 172 wait(0.005);
Khanchana 4:d00236029c9d 173 pos_up_right = pos_up_right + step_up_right;
Khanchana 4:d00236029c9d 174 if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
Khanchana 4:d00236029c9d 175 state_count_right = 3;
Khanchana 0:596de9a5c573 176 }
Khanchana 4:d00236029c9d 177 /*pc.printf("RBD");
Khanchana 4:d00236029c9d 178 pc.printf("%f\n",pos_down_right);
Khanchana 4:d00236029c9d 179 pc.printf("RBP");
Khanchana 4:d00236029c9d 180 pc.printf("%f\n",pos_up_right);*/
Khanchana 4:d00236029c9d 181 } else if(state_count_right == 3) {
Khanchana 4:d00236029c9d 182 Servo3.SetPosition(pos_down_right);
Khanchana 4:d00236029c9d 183 wait(0.005);
Khanchana 4:d00236029c9d 184 pos_down_right = pos_down_right - step_down_right;
Khanchana 4:d00236029c9d 185 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
Khanchana 4:d00236029c9d 186 state_count_right = 4;
Khanchana 1:da79761252bc 187 }
Khanchana 4:d00236029c9d 188 /*pc.printf("RCD");
Khanchana 4:d00236029c9d 189 pc.printf("%f\n",pos_down_right);
Khanchana 4:d00236029c9d 190 pc.printf("RCP");
Khanchana 4:d00236029c9d 191 pc.printf("%f\n",pos_up_right);*/
Khanchana 4:d00236029c9d 192 } else if(state_count_right == 4) {
Khanchana 4:d00236029c9d 193 Servo4.SetPosition(pos_up_right);
Khanchana 4:d00236029c9d 194 wait(0.005);
Khanchana 4:d00236029c9d 195 pos_up_right = pos_up_right - step_up_right;
Khanchana 4:d00236029c9d 196 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) {
Khanchana 4:d00236029c9d 197 state_count_right = 0;
Khanchana 3:64bfd330beb4 198 }
Khanchana 4:d00236029c9d 199 /*pc.printf("RDD");
Khanchana 4:d00236029c9d 200 pc.printf("%f\n",pos_down_right);
Khanchana 4:d00236029c9d 201 pc.printf("RDP");
Khanchana 4:d00236029c9d 202 pc.printf("%f\n",pos_up_right);*/
Khanchana 4:d00236029c9d 203 } else if (state_count_right == 0 and round_count_right < round) {
Khanchana 4:d00236029c9d 204 round_count_right = round_count_right+1;
Khanchana 4:d00236029c9d 205 state_count_right = 1;
Khanchana 4:d00236029c9d 206 pos_down_right = pos_down_start;
Khanchana 4:d00236029c9d 207 pos_up_right = pos_up_start;
Khanchana 4:d00236029c9d 208 }
Khanchana 3:64bfd330beb4 209 }