Diff: ServoClass.h

Revision:

0:14b64813c04d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ServoClass.h	Wed Jun 29 00:02:00 2016 +0000
@@ -0,0 +1,241 @@
+PwmOut PWMservo1(p21);
+PwmOut PWMservo2(p22);
+PwmOut PWMservo3(p23);
+PwmOut PWMservo4(p24);
+
+class Servo
+{
+
+public:
+
+    Servo(int, int, int, int, int, int, int);
+    int getPWforPosMax (int);
+    int getPWforPosIdle (void);
+    int getPWforPosMin (int);
+    int getPWforPosLow (int);
+
+    int offset;
+    int cor;
+    int diffForPosMin;
+    int diffForPosLow;
+    int diffForPosMax;
+    int idlePW;
+    int PW;
+    int reverse_true;
+
+};
+
+Servo::Servo (int of, int mn, int mx, int lw, int idl, int cr, int rev)
+{
+    offset = of;
+    cor = cr;
+    diffForPosMin = mn;
+    diffForPosMax = mx;
+    diffForPosLow = lw;
+    idlePW = idl;
+    reverse_true = rev;
+}
+
+int Servo::getPWforPosMax (int cntr)
+{
+    if(reverse_true) {
+        return(idlePW + offset - diffForPosMax - cor + cntr);
+    } else {
+        return(idlePW + offset + diffForPosMax + cor - cntr);
+    }
+}
+
+int Servo::getPWforPosLow (int cntr_)
+{
+    if(reverse_true) {
+        return(idlePW + offset + diffForPosLow - cor - cntr_);
+    } else {
+        return(idlePW + offset - diffForPosLow + cor + cntr_);
+    }
+}
+
+int Servo::getPWforPosMin (int cntr_)
+{
+    if(reverse_true) {
+        return(idlePW + offset + diffForPosMin - cor - cntr_);
+    } else {
+        return(idlePW + offset - diffForPosMin + cor + cntr_);
+    }
+}
+
+int Servo::getPWforPosIdle ()
+{
+    if(reverse_true) {
+        return(idlePW + offset - cor);
+    } else {
+        return(idlePW + offset + cor);
+    }
+}
+
+Servo Servo_1(0, 200, 250, 100, 1500, 0, 0);
+Servo Servo_2(0, 200, 250, 100, 1500, 0, 1);
+Servo Servo_3(0, 200, 250, 100, 1500, 0, 0);
+Servo Servo_4(0, 200, 250, 100, 1500, 0, 1);
+
+//Servo Servo_1(100, 200, 250, 100, 1500, 0, 0);        OLD VALUES (OFFSET)
+//Servo Servo_2(30, 200, 250, 100, 1500, 0, 1);
+//Servo Servo_3(-10, 200, 250, 100, 1500, 0, 0);
+//Servo Servo_4(-120, 200, 250, 100, 1500, 0, 1);
+
+void movePosTop(void)
+{
+    PWMservo1.pulsewidth_us(Servo_1.getPWforPosMax(0));
+    PWMservo2.pulsewidth_us(Servo_2.getPWforPosMax(0));
+    PWMservo3.pulsewidth_us(Servo_3.getPWforPosMax(0));
+    PWMservo4.pulsewidth_us(Servo_4.getPWforPosMax(0));
+}
+
+void movePosTopSlow(void)
+{
+    int cntr_ = 400;
+    while(cntr_ > 0) {
+        wait(0.0005);
+        PWMservo1.pulsewidth_us(Servo_1.getPWforPosMax(cntr_));
+        PWMservo2.pulsewidth_us(Servo_2.getPWforPosMax(cntr_));
+        PWMservo3.pulsewidth_us(Servo_3.getPWforPosMax(cntr_));
+        PWMservo4.pulsewidth_us(Servo_4.getPWforPosMax(cntr_));
+        cntr_--;
+    }
+}
+
+void movePosLowSlow(void)
+{
+    int cntr_ = 400;
+    while(cntr_ > 0) {
+        wait(0.001);
+        PWMservo1.pulsewidth_us(Servo_1.getPWforPosLow(cntr_));
+        PWMservo2.pulsewidth_us(Servo_2.getPWforPosLow(cntr_));
+        PWMservo3.pulsewidth_us(Servo_3.getPWforPosLow(cntr_));
+        PWMservo4.pulsewidth_us(Servo_4.getPWforPosLow(cntr_));
+        cntr_--;
+    }
+}
+
+void movePosLow(void)
+{
+    int cntr_ = 400;
+    while(cntr_ > 0) {
+        wait(0.0003);
+        PWMservo1.pulsewidth_us(Servo_1.getPWforPosLow(cntr_));
+        PWMservo2.pulsewidth_us(Servo_2.getPWforPosLow(cntr_));
+        PWMservo3.pulsewidth_us(Servo_3.getPWforPosLow(cntr_));
+        PWMservo4.pulsewidth_us(Servo_4.getPWforPosLow(cntr_));
+        cntr_--;
+    }
+}
+
+void movePosBottomFast(void)
+{
+    PWMservo1.pulsewidth_us(Servo_1.getPWforPosMin(0));
+    PWMservo2.pulsewidth_us(Servo_2.getPWforPosMin(0));
+    PWMservo3.pulsewidth_us(Servo_3.getPWforPosMin(0));
+    PWMservo4.pulsewidth_us(Servo_4.getPWforPosMin(0));
+}
+
+void movePosBottom(void)
+{
+    int cntr_ = 400;
+    while(cntr_ > 0) {
+        wait(0.0003);
+        PWMservo1.pulsewidth_us(Servo_1.getPWforPosMin(cntr_));
+        PWMservo2.pulsewidth_us(Servo_2.getPWforPosMin(cntr_));
+        PWMservo3.pulsewidth_us(Servo_3.getPWforPosMin(cntr_));
+        PWMservo4.pulsewidth_us(Servo_4.getPWforPosMin(cntr_));
+        cntr_--;
+    }
+}
+
+void movePosBottomSlow(void)
+{
+    int cntr_ = 400;
+    while(cntr_ > 0) {
+        wait(0.0005);
+        PWMservo1.pulsewidth_us(Servo_1.getPWforPosMin(cntr_));
+        PWMservo2.pulsewidth_us(Servo_2.getPWforPosMin(cntr_));
+        PWMservo3.pulsewidth_us(Servo_3.getPWforPosMin(cntr_));
+        PWMservo4.pulsewidth_us(Servo_4.getPWforPosMin(cntr_));
+        cntr_--;
+    }
+}
+
+void movePosIdle(void)
+{
+    PWMservo1.pulsewidth_us(Servo_1.getPWforPosIdle());
+    PWMservo2.pulsewidth_us(Servo_2.getPWforPosIdle());
+    PWMservo3.pulsewidth_us(Servo_3.getPWforPosIdle());
+    PWMservo4.pulsewidth_us(Servo_4.getPWforPosIdle());
+
+}
+
+void correctPlateToLeft(int val)
+{
+    int corVal = val;
+    Servo_1.cor += -corVal;
+    Servo_4.cor += -corVal;
+    Servo_2.cor +=  corVal;
+    Servo_3.cor +=  corVal;
+}
+
+void correctPlateToRight(int val)
+{
+    int corVal = val;
+    Servo_2.cor += -corVal;
+    Servo_3.cor += -corVal;
+    Servo_1.cor +=  corVal;
+    Servo_4.cor +=  corVal;
+}
+
+void correctPlateToFront(int val)
+{
+    int corVal = val;
+    Servo_1.cor += -corVal;
+    Servo_2.cor += -corVal;
+    Servo_3.cor += corVal;
+    Servo_4.cor += corVal;
+}
+
+void correctPlateToBack(int val)
+{
+    int corVal = val;
+    Servo_1.cor +=  corVal;
+    Servo_2.cor +=  corVal;
+    Servo_3.cor += -corVal;
+    Servo_4.cor += -corVal;
+}
+
+void resetCorrection(void)
+{
+    Servo_2.cor =  0;
+    Servo_3.cor =  0;
+    Servo_1.cor =  0;
+    Servo_4.cor =  0;
+}
+
+
+void UpDownx2(void)
+{
+    movePosTopSlow();
+    wait(0.1);
+    movePosBottomSlow();
+    wait(0.1);
+    movePosTopSlow();
+    wait(0.1);
+    movePosBottomSlow();
+    wait(0.1);
+}
+
+
+
+void initServos()
+{
+    PWMservo1.period(0.004);          // servo requires a 20ms period
+    PWMservo2.period(0.004);
+    PWMservo3.period(0.004);
+    PWMservo4.period(0.004);
+}
+