Tobias Kuhn / Mbed 2 deprecated LCD_XpressoBoard_3_SoundSens

program for ping pong robot

Dependencies:   mbed

ServoClass.h@0:14b64813c04d, 2016-06-29 (annotated)

Committer:
pnpako
Date:
Wed Jun 29 00:02:00 2016 +0000
Revision:
0:14b64813c04d
ping pong robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pnpako 0:14b64813c04d 1 PwmOut PWMservo1(p21);
pnpako 0:14b64813c04d 2 PwmOut PWMservo2(p22);
pnpako 0:14b64813c04d 3 PwmOut PWMservo3(p23);
pnpako 0:14b64813c04d 4 PwmOut PWMservo4(p24);
pnpako 0:14b64813c04d 5
pnpako 0:14b64813c04d 6 class Servo
pnpako 0:14b64813c04d 7 {
pnpako 0:14b64813c04d 8
pnpako 0:14b64813c04d 9 public:
pnpako 0:14b64813c04d 10
pnpako 0:14b64813c04d 11 Servo(int, int, int, int, int, int, int);
pnpako 0:14b64813c04d 12 int getPWforPosMax (int);
pnpako 0:14b64813c04d 13 int getPWforPosIdle (void);
pnpako 0:14b64813c04d 14 int getPWforPosMin (int);
pnpako 0:14b64813c04d 15 int getPWforPosLow (int);
pnpako 0:14b64813c04d 16
pnpako 0:14b64813c04d 17 int offset;
pnpako 0:14b64813c04d 18 int cor;
pnpako 0:14b64813c04d 19 int diffForPosMin;
pnpako 0:14b64813c04d 20 int diffForPosLow;
pnpako 0:14b64813c04d 21 int diffForPosMax;
pnpako 0:14b64813c04d 22 int idlePW;
pnpako 0:14b64813c04d 23 int PW;
pnpako 0:14b64813c04d 24 int reverse_true;
pnpako 0:14b64813c04d 25
pnpako 0:14b64813c04d 26 };
pnpako 0:14b64813c04d 27
pnpako 0:14b64813c04d 28 Servo::Servo (int of, int mn, int mx, int lw, int idl, int cr, int rev)
pnpako 0:14b64813c04d 29 {
pnpako 0:14b64813c04d 30 offset = of;
pnpako 0:14b64813c04d 31 cor = cr;
pnpako 0:14b64813c04d 32 diffForPosMin = mn;
pnpako 0:14b64813c04d 33 diffForPosMax = mx;
pnpako 0:14b64813c04d 34 diffForPosLow = lw;
pnpako 0:14b64813c04d 35 idlePW = idl;
pnpako 0:14b64813c04d 36 reverse_true = rev;
pnpako 0:14b64813c04d 37 }
pnpako 0:14b64813c04d 38
pnpako 0:14b64813c04d 39 int Servo::getPWforPosMax (int cntr)
pnpako 0:14b64813c04d 40 {
pnpako 0:14b64813c04d 41 if(reverse_true) {
pnpako 0:14b64813c04d 42 return(idlePW + offset - diffForPosMax - cor + cntr);
pnpako 0:14b64813c04d 43 } else {
pnpako 0:14b64813c04d 44 return(idlePW + offset + diffForPosMax + cor - cntr);
pnpako 0:14b64813c04d 45 }
pnpako 0:14b64813c04d 46 }
pnpako 0:14b64813c04d 47
pnpako 0:14b64813c04d 48 int Servo::getPWforPosLow (int cntr_)
pnpako 0:14b64813c04d 49 {
pnpako 0:14b64813c04d 50 if(reverse_true) {
pnpako 0:14b64813c04d 51 return(idlePW + offset + diffForPosLow - cor - cntr_);
pnpako 0:14b64813c04d 52 } else {
pnpako 0:14b64813c04d 53 return(idlePW + offset - diffForPosLow + cor + cntr_);
pnpako 0:14b64813c04d 54 }
pnpako 0:14b64813c04d 55 }
pnpako 0:14b64813c04d 56
pnpako 0:14b64813c04d 57 int Servo::getPWforPosMin (int cntr_)
pnpako 0:14b64813c04d 58 {
pnpako 0:14b64813c04d 59 if(reverse_true) {
pnpako 0:14b64813c04d 60 return(idlePW + offset + diffForPosMin - cor - cntr_);
pnpako 0:14b64813c04d 61 } else {
pnpako 0:14b64813c04d 62 return(idlePW + offset - diffForPosMin + cor + cntr_);
pnpako 0:14b64813c04d 63 }
pnpako 0:14b64813c04d 64 }
pnpako 0:14b64813c04d 65
pnpako 0:14b64813c04d 66 int Servo::getPWforPosIdle ()
pnpako 0:14b64813c04d 67 {
pnpako 0:14b64813c04d 68 if(reverse_true) {
pnpako 0:14b64813c04d 69 return(idlePW + offset - cor);
pnpako 0:14b64813c04d 70 } else {
pnpako 0:14b64813c04d 71 return(idlePW + offset + cor);
pnpako 0:14b64813c04d 72 }
pnpako 0:14b64813c04d 73 }
pnpako 0:14b64813c04d 74
pnpako 0:14b64813c04d 75 Servo Servo_1(0, 200, 250, 100, 1500, 0, 0);
pnpako 0:14b64813c04d 76 Servo Servo_2(0, 200, 250, 100, 1500, 0, 1);
pnpako 0:14b64813c04d 77 Servo Servo_3(0, 200, 250, 100, 1500, 0, 0);
pnpako 0:14b64813c04d 78 Servo Servo_4(0, 200, 250, 100, 1500, 0, 1);
pnpako 0:14b64813c04d 79
pnpako 0:14b64813c04d 80 //Servo Servo_1(100, 200, 250, 100, 1500, 0, 0); OLD VALUES (OFFSET)
pnpako 0:14b64813c04d 81 //Servo Servo_2(30, 200, 250, 100, 1500, 0, 1);
pnpako 0:14b64813c04d 82 //Servo Servo_3(-10, 200, 250, 100, 1500, 0, 0);
pnpako 0:14b64813c04d 83 //Servo Servo_4(-120, 200, 250, 100, 1500, 0, 1);
pnpako 0:14b64813c04d 84
pnpako 0:14b64813c04d 85 void movePosTop(void)
pnpako 0:14b64813c04d 86 {
pnpako 0:14b64813c04d 87 PWMservo1.pulsewidth_us(Servo_1.getPWforPosMax(0));
pnpako 0:14b64813c04d 88 PWMservo2.pulsewidth_us(Servo_2.getPWforPosMax(0));
pnpako 0:14b64813c04d 89 PWMservo3.pulsewidth_us(Servo_3.getPWforPosMax(0));
pnpako 0:14b64813c04d 90 PWMservo4.pulsewidth_us(Servo_4.getPWforPosMax(0));
pnpako 0:14b64813c04d 91 }
pnpako 0:14b64813c04d 92
pnpako 0:14b64813c04d 93 void movePosTopSlow(void)
pnpako 0:14b64813c04d 94 {
pnpako 0:14b64813c04d 95 int cntr_ = 400;
pnpako 0:14b64813c04d 96 while(cntr_ > 0) {
pnpako 0:14b64813c04d 97 wait(0.0005);
pnpako 0:14b64813c04d 98 PWMservo1.pulsewidth_us(Servo_1.getPWforPosMax(cntr_));
pnpako 0:14b64813c04d 99 PWMservo2.pulsewidth_us(Servo_2.getPWforPosMax(cntr_));
pnpako 0:14b64813c04d 100 PWMservo3.pulsewidth_us(Servo_3.getPWforPosMax(cntr_));
pnpako 0:14b64813c04d 101 PWMservo4.pulsewidth_us(Servo_4.getPWforPosMax(cntr_));
pnpako 0:14b64813c04d 102 cntr_--;
pnpako 0:14b64813c04d 103 }
pnpako 0:14b64813c04d 104 }
pnpako 0:14b64813c04d 105
pnpako 0:14b64813c04d 106 void movePosLowSlow(void)
pnpako 0:14b64813c04d 107 {
pnpako 0:14b64813c04d 108 int cntr_ = 400;
pnpako 0:14b64813c04d 109 while(cntr_ > 0) {
pnpako 0:14b64813c04d 110 wait(0.001);
pnpako 0:14b64813c04d 111 PWMservo1.pulsewidth_us(Servo_1.getPWforPosLow(cntr_));
pnpako 0:14b64813c04d 112 PWMservo2.pulsewidth_us(Servo_2.getPWforPosLow(cntr_));
pnpako 0:14b64813c04d 113 PWMservo3.pulsewidth_us(Servo_3.getPWforPosLow(cntr_));
pnpako 0:14b64813c04d 114 PWMservo4.pulsewidth_us(Servo_4.getPWforPosLow(cntr_));
pnpako 0:14b64813c04d 115 cntr_--;
pnpako 0:14b64813c04d 116 }
pnpako 0:14b64813c04d 117 }
pnpako 0:14b64813c04d 118
pnpako 0:14b64813c04d 119 void movePosLow(void)
pnpako 0:14b64813c04d 120 {
pnpako 0:14b64813c04d 121 int cntr_ = 400;
pnpako 0:14b64813c04d 122 while(cntr_ > 0) {
pnpako 0:14b64813c04d 123 wait(0.0003);
pnpako 0:14b64813c04d 124 PWMservo1.pulsewidth_us(Servo_1.getPWforPosLow(cntr_));
pnpako 0:14b64813c04d 125 PWMservo2.pulsewidth_us(Servo_2.getPWforPosLow(cntr_));
pnpako 0:14b64813c04d 126 PWMservo3.pulsewidth_us(Servo_3.getPWforPosLow(cntr_));
pnpako 0:14b64813c04d 127 PWMservo4.pulsewidth_us(Servo_4.getPWforPosLow(cntr_));
pnpako 0:14b64813c04d 128 cntr_--;
pnpako 0:14b64813c04d 129 }
pnpako 0:14b64813c04d 130 }
pnpako 0:14b64813c04d 131
pnpako 0:14b64813c04d 132 void movePosBottomFast(void)
pnpako 0:14b64813c04d 133 {
pnpako 0:14b64813c04d 134 PWMservo1.pulsewidth_us(Servo_1.getPWforPosMin(0));
pnpako 0:14b64813c04d 135 PWMservo2.pulsewidth_us(Servo_2.getPWforPosMin(0));
pnpako 0:14b64813c04d 136 PWMservo3.pulsewidth_us(Servo_3.getPWforPosMin(0));
pnpako 0:14b64813c04d 137 PWMservo4.pulsewidth_us(Servo_4.getPWforPosMin(0));
pnpako 0:14b64813c04d 138 }
pnpako 0:14b64813c04d 139
pnpako 0:14b64813c04d 140 void movePosBottom(void)
pnpako 0:14b64813c04d 141 {
pnpako 0:14b64813c04d 142 int cntr_ = 400;
pnpako 0:14b64813c04d 143 while(cntr_ > 0) {
pnpako 0:14b64813c04d 144 wait(0.0003);
pnpako 0:14b64813c04d 145 PWMservo1.pulsewidth_us(Servo_1.getPWforPosMin(cntr_));
pnpako 0:14b64813c04d 146 PWMservo2.pulsewidth_us(Servo_2.getPWforPosMin(cntr_));
pnpako 0:14b64813c04d 147 PWMservo3.pulsewidth_us(Servo_3.getPWforPosMin(cntr_));
pnpako 0:14b64813c04d 148 PWMservo4.pulsewidth_us(Servo_4.getPWforPosMin(cntr_));
pnpako 0:14b64813c04d 149 cntr_--;
pnpako 0:14b64813c04d 150 }
pnpako 0:14b64813c04d 151 }
pnpako 0:14b64813c04d 152
pnpako 0:14b64813c04d 153 void movePosBottomSlow(void)
pnpako 0:14b64813c04d 154 {
pnpako 0:14b64813c04d 155 int cntr_ = 400;
pnpako 0:14b64813c04d 156 while(cntr_ > 0) {
pnpako 0:14b64813c04d 157 wait(0.0005);
pnpako 0:14b64813c04d 158 PWMservo1.pulsewidth_us(Servo_1.getPWforPosMin(cntr_));
pnpako 0:14b64813c04d 159 PWMservo2.pulsewidth_us(Servo_2.getPWforPosMin(cntr_));
pnpako 0:14b64813c04d 160 PWMservo3.pulsewidth_us(Servo_3.getPWforPosMin(cntr_));
pnpako 0:14b64813c04d 161 PWMservo4.pulsewidth_us(Servo_4.getPWforPosMin(cntr_));
pnpako 0:14b64813c04d 162 cntr_--;
pnpako 0:14b64813c04d 163 }
pnpako 0:14b64813c04d 164 }
pnpako 0:14b64813c04d 165
pnpako 0:14b64813c04d 166 void movePosIdle(void)
pnpako 0:14b64813c04d 167 {
pnpako 0:14b64813c04d 168 PWMservo1.pulsewidth_us(Servo_1.getPWforPosIdle());
pnpako 0:14b64813c04d 169 PWMservo2.pulsewidth_us(Servo_2.getPWforPosIdle());
pnpako 0:14b64813c04d 170 PWMservo3.pulsewidth_us(Servo_3.getPWforPosIdle());
pnpako 0:14b64813c04d 171 PWMservo4.pulsewidth_us(Servo_4.getPWforPosIdle());
pnpako 0:14b64813c04d 172
pnpako 0:14b64813c04d 173 }
pnpako 0:14b64813c04d 174
pnpako 0:14b64813c04d 175 void correctPlateToLeft(int val)
pnpako 0:14b64813c04d 176 {
pnpako 0:14b64813c04d 177 int corVal = val;
pnpako 0:14b64813c04d 178 Servo_1.cor += -corVal;
pnpako 0:14b64813c04d 179 Servo_4.cor += -corVal;
pnpako 0:14b64813c04d 180 Servo_2.cor += corVal;
pnpako 0:14b64813c04d 181 Servo_3.cor += corVal;
pnpako 0:14b64813c04d 182 }
pnpako 0:14b64813c04d 183
pnpako 0:14b64813c04d 184 void correctPlateToRight(int val)
pnpako 0:14b64813c04d 185 {
pnpako 0:14b64813c04d 186 int corVal = val;
pnpako 0:14b64813c04d 187 Servo_2.cor += -corVal;
pnpako 0:14b64813c04d 188 Servo_3.cor += -corVal;
pnpako 0:14b64813c04d 189 Servo_1.cor += corVal;
pnpako 0:14b64813c04d 190 Servo_4.cor += corVal;
pnpako 0:14b64813c04d 191 }
pnpako 0:14b64813c04d 192
pnpako 0:14b64813c04d 193 void correctPlateToFront(int val)
pnpako 0:14b64813c04d 194 {
pnpako 0:14b64813c04d 195 int corVal = val;
pnpako 0:14b64813c04d 196 Servo_1.cor += -corVal;
pnpako 0:14b64813c04d 197 Servo_2.cor += -corVal;
pnpako 0:14b64813c04d 198 Servo_3.cor += corVal;
pnpako 0:14b64813c04d 199 Servo_4.cor += corVal;
pnpako 0:14b64813c04d 200 }
pnpako 0:14b64813c04d 201
pnpako 0:14b64813c04d 202 void correctPlateToBack(int val)
pnpako 0:14b64813c04d 203 {
pnpako 0:14b64813c04d 204 int corVal = val;
pnpako 0:14b64813c04d 205 Servo_1.cor += corVal;
pnpako 0:14b64813c04d 206 Servo_2.cor += corVal;
pnpako 0:14b64813c04d 207 Servo_3.cor += -corVal;
pnpako 0:14b64813c04d 208 Servo_4.cor += -corVal;
pnpako 0:14b64813c04d 209 }
pnpako 0:14b64813c04d 210
pnpako 0:14b64813c04d 211 void resetCorrection(void)
pnpako 0:14b64813c04d 212 {
pnpako 0:14b64813c04d 213 Servo_2.cor = 0;
pnpako 0:14b64813c04d 214 Servo_3.cor = 0;
pnpako 0:14b64813c04d 215 Servo_1.cor = 0;
pnpako 0:14b64813c04d 216 Servo_4.cor = 0;
pnpako 0:14b64813c04d 217 }
pnpako 0:14b64813c04d 218
pnpako 0:14b64813c04d 219
pnpako 0:14b64813c04d 220 void UpDownx2(void)
pnpako 0:14b64813c04d 221 {
pnpako 0:14b64813c04d 222 movePosTopSlow();
pnpako 0:14b64813c04d 223 wait(0.1);
pnpako 0:14b64813c04d 224 movePosBottomSlow();
pnpako 0:14b64813c04d 225 wait(0.1);
pnpako 0:14b64813c04d 226 movePosTopSlow();
pnpako 0:14b64813c04d 227 wait(0.1);
pnpako 0:14b64813c04d 228 movePosBottomSlow();
pnpako 0:14b64813c04d 229 wait(0.1);
pnpako 0:14b64813c04d 230 }
pnpako 0:14b64813c04d 231
pnpako 0:14b64813c04d 232
pnpako 0:14b64813c04d 233
pnpako 0:14b64813c04d 234 void initServos()
pnpako 0:14b64813c04d 235 {
pnpako 0:14b64813c04d 236 PWMservo1.period(0.004); // servo requires a 20ms period
pnpako 0:14b64813c04d 237 PWMservo2.period(0.004);
pnpako 0:14b64813c04d 238 PWMservo3.period(0.004);
pnpako 0:14b64813c04d 239 PWMservo4.period(0.004);
pnpako 0:14b64813c04d 240 }
pnpako 0:14b64813c04d 241