TurtleBot / Mbed 2 deprecated testLibrary

malin

Dependencies:   Servo mbed mbed-rtos

main.cpp@3:64bfd330beb4, 2018-04-02 (annotated)

Committer:
Khanchana
Date:
Mon Apr 02 11:37:43 2018 +0000
Revision:
3:64bfd330beb4
Parent:
2:68dbcd5277e4
Child:
4:d00236029c9d
servoooooo

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 0:596de9a5c573 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 1:da79761252bc 16
Khanchana 3:64bfd330beb4 17 float pos_down = 1400;
Khanchana 3:64bfd330beb4 18 float pos_up = 1000;
Khanchana 3:64bfd330beb4 19 float pos_down_start = 1400;
Khanchana 3:64bfd330beb4 20 float pos_up_start = 1000;
Khanchana 3:64bfd330beb4 21 float pos_down_end = 1544.44;
Khanchana 3:64bfd330beb4 22 float pos_up_end = 1400;
Khanchana 3:64bfd330beb4 23 float state_count = 1;
Khanchana 3:64bfd330beb4 24 float round_count = 1;
Khanchana 3:64bfd330beb4 25 float step = 5;
Khanchana 3:64bfd330beb4 26 float pos_down_right = 1400;
Khanchana 3:64bfd330beb4 27 float pos_up_right = 1000;
Khanchana 3:64bfd330beb4 28 float pos_down_start_right = 1400;
Khanchana 3:64bfd330beb4 29 float pos_up_start_right = 1000;
Khanchana 3:64bfd330beb4 30 float pos_down_end_right = 1544.44;
Khanchana 3:64bfd330beb4 31 float pos_up_end_right = 1500;
Khanchana 3:64bfd330beb4 32 float state_count_right = 1;
Khanchana 3:64bfd330beb4 33 float round_count_right = 1;
Khanchana 3:64bfd330beb4 34 float step_right = 6.25;
Khanchana 2:68dbcd5277e4 35
Khanchana 3:64bfd330beb4 36 int main(){
Khanchana 3:64bfd330beb4 37 while(1){
Khanchana 3:64bfd330beb4 38 wait(5);
Khanchana 3:64bfd330beb4 39 move();
Khanchana 3:64bfd330beb4 40 }
Khanchana 3:64bfd330beb4 41 }
Khanchana 3:64bfd330beb4 42
Khanchana 3:64bfd330beb4 43 void move() {
Khanchana 2:68dbcd5277e4 44 pc.baud(9600);
Khanchana 2:68dbcd5277e4 45 Servo1.Enable(1000,2000);
Khanchana 2:68dbcd5277e4 46 Servo2.Enable(1000,2000);
Khanchana 2:68dbcd5277e4 47 Servo3.Enable(1000,2000);
Khanchana 2:68dbcd5277e4 48 Servo4.Enable(1000,2000);
Khanchana 1:da79761252bc 49 while(1){
Khanchana 3:64bfd330beb4 50 servo_Right();
Khanchana 3:64bfd330beb4 51 if(state_count == 1){
Khanchana 2:68dbcd5277e4 52 Servo1.SetPosition(pos_down);
Khanchana 3:64bfd330beb4 53 //wait(0.001);
Khanchana 3:64bfd330beb4 54 pos_down = pos_down + step;
Khanchana 3:64bfd330beb4 55 if(pos_down >= pos_down_end + step and pos_up == pos_up_start){
Khanchana 3:64bfd330beb4 56 state_count = 2;
Khanchana 3:64bfd330beb4 57 }
Khanchana 3:64bfd330beb4 58 pc.printf("LAD");
Khanchana 3:64bfd330beb4 59 pc.printf("%f\n",pos_down);
Khanchana 3:64bfd330beb4 60 pc.printf("LAP");
Khanchana 3:64bfd330beb4 61 pc.printf("%f\n",pos_up);
Khanchana 1:da79761252bc 62 }
Khanchana 3:64bfd330beb4 63 else if(state_count == 2){
Khanchana 2:68dbcd5277e4 64 Servo2.SetPosition(pos_up);
Khanchana 3:64bfd330beb4 65 //wait(0.001);
Khanchana 3:64bfd330beb4 66 pos_up = pos_up + step;
Khanchana 3:64bfd330beb4 67 if(pos_down >= pos_down_end + step and pos_up >= pos_up_end + step){
Khanchana 3:64bfd330beb4 68 state_count = 3;
Khanchana 3:64bfd330beb4 69 }
Khanchana 3:64bfd330beb4 70 pc.printf("LBD");
Khanchana 3:64bfd330beb4 71 pc.printf("%f\n",pos_down);
Khanchana 3:64bfd330beb4 72 pc.printf("LBP");
Khanchana 3:64bfd330beb4 73 pc.printf("%f\n",pos_up);
Khanchana 1:da79761252bc 74 }
Khanchana 3:64bfd330beb4 75 else if(state_count == 3){
Khanchana 2:68dbcd5277e4 76 Servo1.SetPosition(pos_down);
Khanchana 3:64bfd330beb4 77 //wait(0.001);
Khanchana 3:64bfd330beb4 78 pos_down = pos_down - step;
Khanchana 3:64bfd330beb4 79 if(pos_down <= pos_down_start - step and pos_up >= pos_up_end + step){
Khanchana 3:64bfd330beb4 80 state_count = 4;
Khanchana 3:64bfd330beb4 81 }
Khanchana 3:64bfd330beb4 82 pc.printf("LCD");
Khanchana 3:64bfd330beb4 83 pc.printf("%f\n",pos_down);
Khanchana 3:64bfd330beb4 84 pc.printf("LCP");
Khanchana 3:64bfd330beb4 85 pc.printf("%f\n",pos_up);
Khanchana 1:da79761252bc 86 }
Khanchana 3:64bfd330beb4 87 else if(state_count == 4){
Khanchana 2:68dbcd5277e4 88 Servo2.SetPosition(pos_up);
Khanchana 3:64bfd330beb4 89 //wait(0.001);
Khanchana 3:64bfd330beb4 90 pos_up = pos_up - step;
Khanchana 3:64bfd330beb4 91 if(pos_down <= pos_down_start - step and pos_up <= pos_up_start - step){
Khanchana 3:64bfd330beb4 92 state_count = 0;
Khanchana 3:64bfd330beb4 93 }
Khanchana 3:64bfd330beb4 94 pc.printf("LDD");
Khanchana 3:64bfd330beb4 95 pc.printf("%f\n",pos_down);
Khanchana 3:64bfd330beb4 96 pc.printf("LDP");
Khanchana 3:64bfd330beb4 97 pc.printf("%f\n",pos_up);
Khanchana 3:64bfd330beb4 98 }
Khanchana 3:64bfd330beb4 99 else if (state_count == 0 and round_count < 1){
Khanchana 3:64bfd330beb4 100 round_count = round_count+1;
Khanchana 3:64bfd330beb4 101 state_count = 1;
Khanchana 3:64bfd330beb4 102 pos_down = 1400;
Khanchana 3:64bfd330beb4 103 pos_up = 1000;
Khanchana 3:64bfd330beb4 104 }
Khanchana 1:da79761252bc 105 }
Khanchana 1:da79761252bc 106 }
Khanchana 2:68dbcd5277e4 107
Khanchana 3:64bfd330beb4 108 void servo_Right(){
Khanchana 3:64bfd330beb4 109 if(state_count_right == 1){
Khanchana 3:64bfd330beb4 110 Servo3.SetPosition(pos_down_right);
Khanchana 3:64bfd330beb4 111 //wait(0.001);
Khanchana 3:64bfd330beb4 112 pos_down_right = pos_down_right + step;
Khanchana 3:64bfd330beb4 113 if(pos_down_right >= pos_down_end_right + step and pos_up_right == pos_up_start){
Khanchana 3:64bfd330beb4 114 state_count_right = 2;
Khanchana 3:64bfd330beb4 115 }
Khanchana 3:64bfd330beb4 116 pc.printf("RAD");
Khanchana 3:64bfd330beb4 117 pc.printf("%f\n",pos_down_right);
Khanchana 3:64bfd330beb4 118 pc.printf("RAP");
Khanchana 3:64bfd330beb4 119 pc.printf("%f\n",pos_up_right);
Khanchana 0:596de9a5c573 120 }
Khanchana 3:64bfd330beb4 121 else if(state_count_right == 2){
Khanchana 3:64bfd330beb4 122 Servo4.SetPosition(pos_up_right);
Khanchana 3:64bfd330beb4 123 //wait(0.001);
Khanchana 3:64bfd330beb4 124 pos_up_right = pos_up_right + step_right;
Khanchana 3:64bfd330beb4 125 if(pos_down_right >= pos_down_end_right + step and pos_up_right >= pos_up_end_right + step_right){
Khanchana 3:64bfd330beb4 126 state_count_right = 3;
Khanchana 3:64bfd330beb4 127 }
Khanchana 3:64bfd330beb4 128 pc.printf("RBD");
Khanchana 3:64bfd330beb4 129 pc.printf("%f\n",pos_down_right);
Khanchana 3:64bfd330beb4 130 pc.printf("RBP");
Khanchana 3:64bfd330beb4 131 pc.printf("%f\n",pos_up_right);
Khanchana 0:596de9a5c573 132 }
Khanchana 3:64bfd330beb4 133 else if(state_count_right == 3){
Khanchana 3:64bfd330beb4 134 Servo3.SetPosition(pos_down_right);
Khanchana 3:64bfd330beb4 135 //wait(0.001);
Khanchana 3:64bfd330beb4 136 pos_down_right = pos_down_right - step;
Khanchana 3:64bfd330beb4 137 if(pos_down_right <= pos_down_start_right - step and pos_up_right >= pos_up_end_right + step_right){
Khanchana 3:64bfd330beb4 138 state_count_right = 4;
Khanchana 3:64bfd330beb4 139 }
Khanchana 3:64bfd330beb4 140 pc.printf("RCD");
Khanchana 3:64bfd330beb4 141 pc.printf("%f\n",pos_down_right);
Khanchana 3:64bfd330beb4 142 pc.printf("RCP");
Khanchana 3:64bfd330beb4 143 pc.printf("%f\n",pos_up_right);
Khanchana 1:da79761252bc 144 }
Khanchana 3:64bfd330beb4 145 else if(state_count_right == 4){
Khanchana 3:64bfd330beb4 146 Servo4.SetPosition(pos_up_right);
Khanchana 3:64bfd330beb4 147 //wait(0.001);
Khanchana 3:64bfd330beb4 148 pos_up_right = pos_up_right - step_right;
Khanchana 3:64bfd330beb4 149 if(pos_down_right <= pos_down_start_right - step and pos_up_right <= pos_up_start_right - step_right){
Khanchana 3:64bfd330beb4 150 state_count_right = 0;
Khanchana 3:64bfd330beb4 151 }
Khanchana 3:64bfd330beb4 152 pc.printf("RDD");
Khanchana 3:64bfd330beb4 153 pc.printf("%f\n",pos_down_right);
Khanchana 3:64bfd330beb4 154 pc.printf("RDP");
Khanchana 3:64bfd330beb4 155 pc.printf("%f\n",pos_up_right);
Khanchana 3:64bfd330beb4 156 }
Khanchana 3:64bfd330beb4 157 else if (state_count_right == 0 and round_count_right < 1){
Khanchana 3:64bfd330beb4 158 round_count_right = round_count_right+1;
Khanchana 3:64bfd330beb4 159 state_count_right = 1;
Khanchana 3:64bfd330beb4 160 pos_down_right = 1400;
Khanchana 3:64bfd330beb4 161 pos_up_right = 1000;
Khanchana 3:64bfd330beb4 162 }
Khanchana 3:64bfd330beb4 163 }