Saturday November 25 2017

Arm Control GUI Processing Code

// ***  ALL MY Code is 100% free of cost and liability  ***
//-------------------------------------------------------------------------------//
// Global
float ScreenHeight = 768*.9;
float ScreenWidth  = 1024*1.75;
boolean SomethingWentWrong = false;
// Serial
boolean SerialEnabled = true;
String inString = "";
import processing.serial.*;
Serial myPort;
// WiiRemote
import lll.wrj4P5.*;
import lll.Loc.*;
boolean WiiRemoteEnabled = true;
WiiPointList WiiRemote;
// Constraints
ConstraintsArrayList ConstraintsLinesArray = new ConstraintsArrayList();
Point ConstraintPoint;
boolean OneConstraintPointDrawn = false;
// Arm
boolean ArmStarted = false;
boolean AutoAlignArmPointsToExtra = false;
boolean MouseCanUpdatePoints = false;
int PointMouseWillUpdate = 0;
ArmSegmentChain RobotArm = new ArmSegmentChain();
// Debug
Point TestPoint = new Point(-100,-100);
//-------------------------------------------------------------------------------//
void setup() {
    size(floor(ScreenWidth),floor(ScreenHeight));
    // create a font with the third font available to the system:
    PFont myFont = createFont(PFont.list()[2], 20);
    textFont(myFont);
    // Serial
    if(SerialEnabled) {
        println("Available ports are:");
        println(Serial.list());
        myPort = new Serial(this, Serial.list()[0], 9600);
    }
    // WiiRemote
    if(WiiRemoteEnabled) WiiRemote = new WiiPointList(this,4);
    else                 WiiRemote = new WiiPointList(ScreenWidth*2./3.,ScreenHeight*2./3.); // 2 float Args is for a skelleton (they make origin)
    // Robot Arm
    RobotArm.InitPoints();
}
//-------------------------------------------------------------------------------//
void draw() {
    //-----------Draw Prepare------------//
    background(0);
    if(RobotArm.ConstantPressureUpdate) {
        RobotArm.ChangingHandState = true; // To see echo
        myPort.write("B_1_0_0_0_1_E"); // To trigger the echo
    }
    if(keyPressed && (key == 'E')) {
        WiiRemote.ExtraPoint = new Point(mouseX,mouseY,20);
        WiiRemote.PointMemorized = 4;
    }
    // Update FeedBack Points
    if(WiiRemoteEnabled) WiiRemote.UpdatePoints(ScreenWidth/3.,ScreenHeight);//*2./3.);
    // Update RobotArm Points if needed
    if(AutoAlignArmPointsToExtra) RobotArm.AlignHandToPoint(WiiRemote.ExtraPoint,WiiRemote.Origin);
    //-----------Begin Draw------------//
    strokeWeight(16);
    // WiiRemote
    WiiRemote.DrawAll(255,128,0);
    // RobotArm
    RobotArm.DrawAll(25,124,181);
    if(MouseCanUpdatePoints) RobotArm.Points[PointMouseWillUpdate].DrawOX(0,255,0,40);
    // Constraints
    ConstraintsLinesArray.DrawAll(255,0,0);
    if(OneConstraintPointDrawn) {
        stroke(0,0,255);
        fill(0,0,255);
        line(ConstraintPoint.X,ConstraintPoint.Y,mouseX,mouseY);
    }
    TestPoint.DrawOX(0,255,0,40);

    //---------- Global Settings Echo ----------//
    stroke(69,88,148);
    fill(  69,88,148);
    text("AutoAlignArmPointsToExtra:" + AutoAlignArmPointsToExtra,20,20);
    if(SomethingWentWrong) {
        stroke(255,0,0);
        fill(  255,0,0);
        text("SomethingWentWrong!",20,50);
    }
    //---------- Movement ----------//
    if(RobotArm.EnableArmUpdate&&(!SomethingWentWrong)) RobotArm.MoveTowardsAngles(WiiRemote.Angles, WiiRemote.ViolatingSegments, 3);
}
//-------------------------------------------------------------------------------//
void mousePressed(){
    if(MouseCanUpdatePoints) {
        RobotArm.Points[PointMouseWillUpdate] = new Point(mouseX,mouseY,20);
        RobotArm.BuildArmSegments();
    } else {
        if(keyPressed && key=='C') { // Draw constriant point
            // Append line to constraints or determin ConstraintPoint
            if(!OneConstraintPointDrawn) {
                OneConstraintPointDrawn = true;
                ConstraintPoint = new Point(mouseX,mouseY);
            } else {
                OneConstraintPointDrawn = false;
                Point ConstraintPoint_2 = new Point(mouseX,mouseY);
                Line ConstraintLine = new Line(ConstraintPoint,ConstraintPoint_2);
                ConstraintsLinesArray.add(ConstraintLine);
            }
        } else {
            TestPoint = new Point(mouseX,mouseY,20);
        }
    }
}
//-------------------------------------------------------------------------------//
void keyPressed() {
    // RobotArm related
    if     ((key == '1')&&(RobotArm.Motors[0] < 255))  RobotArm.Motors[0] += 5;
    else if((key == 'q')&&(RobotArm.Motors[0] > -255)) RobotArm.Motors[0] -= 5;
    else if((key == 'a'))                              RobotArm.Motors[0] = 0;
    else if((key == 'z'))                              RobotArm.Motors[0] *= -1;
    else if((key == '2')&&(RobotArm.Motors[1] < 255))  RobotArm.Motors[1] += 5;
    else if((key == 'w')&&(RobotArm.Motors[1] > -255)) RobotArm.Motors[1] -= 5;
    else if((key == 's'))                              RobotArm.Motors[1] = 0;
    else if((key == 'x'))                              RobotArm.Motors[1] *= -1;
    else if((key == '3')&&(RobotArm.Motors[2] < 255))  RobotArm.Motors[2] += 5;
    else if((key == 'e')&&(RobotArm.Motors[2] > -255)) RobotArm.Motors[2] -= 5;
    else if((key == 'd'))                              RobotArm.Motors[2] = 0;
    else if((key == 'c'))                              RobotArm.Motors[2] *= -1;
    else if((key == '4')&&(RobotArm.Motors[3] < 255))  RobotArm.Motors[3] += 5;
    else if((key == 'r')&&(RobotArm.Motors[3] > -255)) RobotArm.Motors[3] -= 5;
    else if((key == 'f'))                              RobotArm.Motors[3] = 0;
    else if((key == 'v'))                              RobotArm.Motors[3] *= -1;
    else if((key == ' ')&&SerialEnabled)                   RobotArm.RunMotors();
    else if((key == ']')&&(RobotArm.MagicDuration < 500))  RobotArm.MagicDuration += 10;
    else if((key == '[')&&(RobotArm.MagicDuration > 10))   RobotArm.MagicDuration -= 10;
    else if((key == '}')&&(RobotArm.MagicThreshold < 255)) RobotArm.MagicThreshold += 5;
    else if((key == '{')&&(RobotArm.MagicThreshold > 5))   RobotArm.MagicThreshold -= 5;
    else if((key == '+')&&(RobotArm.OnPressure < 255))     RobotArm.OnPressure += 5;
    else if((key == '-')&&(RobotArm.OnPressure > 5))       RobotArm.OnPressure -= 5;
    else if((key == CODED)&&(keyCode == RIGHT)&&(RobotArm.Duration < 1000)) RobotArm.Duration += 10;
    else if((key == CODED)&&(keyCode == LEFT)&&(RobotArm.Duration > 10))    RobotArm.Duration -= 10;
    else if(key == ')')  RobotArm.Damper += 0.25;
    else if(key == '(')  RobotArm.Damper -= 0.25;
    else if(key == 'i')  RobotArm.InitPoints();
    else if(key == 'l')  RobotArm.SetPoints(WiiRemote.Points,WiiRemote.Arm.seg2_p1_distToEdge);
    else if(key == '>')  RobotArm.CloseHand();
    else if(key == '<')  RobotArm.OpenHand();
    else if(key == 'o')  RobotArm.OpenHandSmallBit();
    else if(key == '.')  RobotArm.MoveTowardsAngles(WiiRemote.Angles, WiiRemote.ViolatingSegments, 3);
    else if(key == 'k')  RobotArm.MoveTowardsPPP = (RobotArm.MoveTowardsPPP+1)%5;
    else if(key == 'K')  RobotArm.MoveTowardsPPP = (RobotArm.MoveTowardsPPP+4)%5;  // Cuz 4==-1  %5
    else if(key == '?')  RobotArm.ConstantPressureUpdate = !RobotArm.ConstantPressureUpdate;
    else if(key == 'u')  RobotArm.EnableArmUpdate = !RobotArm.EnableArmUpdate;
    else if(key == '\'') RobotArm.MassageAngles = !RobotArm.MassageAngles;
    // WiiRemote related
    else if((key == CODED)&&(keyCode == UP))   WiiRemote.Scale *= 1.1;
    else if((key == CODED)&&(keyCode == DOWN)) WiiRemote.Scale /= 1.1;
    else if(key == 'N')                        WiiRemote.PointMemorized = PointMouseWillUpdate;
    else if(key == 'n')                        WiiRemote.PointMemorized = -1;
    else if(key == 'O')                        WiiRemote.PointMemorized = 0;
    else if(key == 'h')                        WiiRemote.ExtraPoint = new Point(WiiRemote.ExtraPoint.X-2*(WiiRemote.ExtraPoint.X-RobotArm.Points[0].X),WiiRemote.ExtraPoint.Y,WiiRemote.ExtraPoint.Z);
    // Other related
    else if(key == '=') AutoAlignArmPointsToExtra = !AutoAlignArmPointsToExtra;
    else if(key == 'W') SomethingWentWrong = false;
    else if(key == 'Z') OneConstraintPointDrawn = false;
    else if(key == 'M') MouseCanUpdatePoints = !MouseCanUpdatePoints;
    else if(key == 'm') {
        if(PointMouseWillUpdate < 3) PointMouseWillUpdate++;
        else                         PointMouseWillUpdate=0;
    }
}
//-------------------------------------------------------------------------------//
void serialEvent(Serial myPort) {
    if(SerialEnabled) {
        char inChar = (char)myPort.read();
        if(inChar != '\n') {
            inString += inChar;  // Keep reading until you hit a \n
        } else {
            if(inString.equals("ArmControlStart")) {
                if(ArmStarted) SomethingWentWrong = true; // Only could see twice+ if reset button hit
                else           ArmStarted = true;
            }
            if(RobotArm.ChangingHandState) {
                // Convert string to int for Pressure
                int [] ParsedInts = ParseIntegerString(inString,'_',2);
                if(ParsedInts[0] == 2) { // There should only be two ints in the string
                    RobotArm.Pressure = ParsedInts[1];
                    RobotArm.Openness = ParsedInts[2];
                    if(RobotArm.DisplayStatus > 2) print("Changing Pressure and Openness to: " + RobotArm.Pressure + ", " + RobotArm.Openness);
                }
            }
            // Regular echo
            println("SerialRead: " + inString);
            // Clear for next use
            inString = "";
        }
    }
}
//-------------------------------------------------------------------------------//
int [] ParseIntegerString(String Input, char Spacer, int numInts) { // Note that returned[0] is the count of integers found
    int [] Buffer = new int[numInts+1]; // numInts + [0] holding the count
    int [] ToReturn;
    int index=1;    // Start at first int
    Buffer[0] = 0; // Start with nothine, invalid==-1
    int temp=0;
    for(int i=0;i < Input.length();i++) {
        char tempc = 0;
        if(i != Input.length()-1) tempc = (char)Input.charAt(i);
        if((tempc>='0')&&(tempc<='9')) {
            temp *= 10; // Base10 shift of accumulator
            temp += (int)(tempc - '0');  // Collect
        } else if((tempc == Spacer)||(tempc == 0)||(index == numInts)) {
            if(index > numInts) {
                index--; // So the right value get's passed on
                break;   // Don't care about any other ints
            } else {
                Buffer[index] = temp;
                temp = 0;
                index++;
            }
        } else {
            println("ParseIntegerString::ERROR:: bad char:" + tempc);
            Buffer[0] = -1;
            break;
        }
    }
    if(Buffer[0] == -1) {
        ToReturn = new int[1];
        ToReturn[0] = -1;
        println("ParseIntegerString::ERROR Occured:");
    } else {
        Buffer[0] = index-1;
        ToReturn = new int[numInts+1]; // index ints + count
        for(int i=0;i < numInts+1;i++) ToReturn[i] = Buffer[i];
        // println("ParseIntegerString::Got this many ints::" + index);
    }
    return ToReturn;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class WiiPointList {
    Point [] Points;
    int PointMemorized = -1;
    Point ExtraPoint = new Point(-10,-10);
    Point CurrRef = new Point(0,0);
    Point Origin = new Point(0,0);
    boolean PointsDefined = false;
    int numPoints = 0;
    float [] Angles;
    float AngleFlex = 30;   // Remember to try to reduce this and see how picky it gets
    boolean [] ViolatingSegments;
    boolean RemoteConnected = false;
    boolean RelativeExtraDistSet = false;
    Wrj4P5 Remote;
    int DisplayStatus = 1;//1;
    float Scale = 1.5;
    ArmSegmentChain Arm;
    //------------------------------------------------------------------//
    WiiPointList(PApplet parent, int count) {
        this.Remote = new Wrj4P5(parent).connect(Wrj4P5.IR);
        this.numPoints = count;
        this.Points = new Point[count];
        for(int i=0;i < count;i++) this.Points[i] = new Point(0,0,0);
        this.Angles = new float[count-1];
        for(int i=0;i < count-1;i++) this.Angles[i] = 0;
        this.ViolatingSegments = new boolean[count-1];
        for(int i=0;i < count-1;i++) this.ViolatingSegments[i] = false;
        this.Arm = new ArmSegmentChain();
        this.Arm.Points = this.Points;
        this.Arm.Angles = this.Angles;
        this.Arm.DisplayStatus = this.DisplayStatus;
    }
    WiiPointList(float x, float y) {
        this.Origin = new Point(x,y);
        this.numPoints = 0;
    }
    //------------------------------------------------------------------//
    void UpdatePoints(float X_origin, float Y_origin) {
        if(!this.RemoteConnected) {
            if(!this.Remote.isConnecting()) {
                this.RemoteConnected = true;
                println("WiiRemote connected!");
            }
        } else {
            this.PointsDefined = true;
            this.Origin = new Point(X_origin,Y_origin);
            for (int i=0;i < this.numPoints;i++) {
                Loc p=this.Remote.rimokon.irLights[i];
                if (p.x > -1) { // Valid point
                    float x = p.x*640*this.Scale;
                    float y = (1.-p.y)*480*this.Scale;
                    float z = p.z*120*this.Scale;
                    if(i == this.PointMemorized) {  // Update memorized point
                        this.ExtraPoint = new Point(x,y,z);
                        this.ExtraPoint.SubtractOffset(this.CurrRef);
                        this.ExtraPoint.AddOffset(this.Origin);
                    } else {
                        if(i == 0) {    // Update current ref
                            this.CurrRef = new Point(x,y,z);
                            this.Points[0] = new Point(X_origin,Y_origin,z);
                        } else {        // Update Point
                            this.Points[i] = new Point(x,y,z);
                            this.Points[i].SubtractOffset(this.CurrRef);
                            this.Points[i].AddOffset(this.Origin);
                        }
                    }
                } else if(i != this.PointMemorized){    // ERROR
                    this.Points[i].Z = 1;
                    this.Points[i].DrawOX(255,0,0,60);
                    this.Points[i].DrawO(255,0,0,40);
                    SomethingWentWrong = true;
                } else {                // Lost memorized point
                    this.Points[i].DrawOX(0,0,255,60);
                }
            }
            if(!this.RelativeExtraDistSet) {
                this.Arm.seg2_p1_distToEdge = 2*this.Points[3].RelativeDist(this.Points[2]);
                this.RelativeExtraDistSet = true;
            }
            this.Arm.UpdateAngles();
            for (int i=0;i<3;i++) if((this.Arm.Angles[i]>(this.Arm.MaxAngles[i]+this.AngleFlex))||(this.Arm.Angles[i]<(this.Arm.MinAngles[i]-this.AngleFlex))) SomethingWentWrong = true;
            this.Arm.BuildArmSegments();
            // Now determin if any segments violate constraints
            for (int i=0;i<3;i++) {
                this.ViolatingSegments[i] = false;
                for (int j=0;j<4;j++) this.ViolatingSegments[i] |= (ConstraintsLinesArray.TestForIntersection(this.Arm.ArmLines[i*4+j]) > 0);
            }
        }
    }
    //------------------------------------------------------------------//
    void DrawAll(int r, int g, int b) {
        int i;
        stroke(r,g,b);
        fill  (r,g,b);
        if(this.PointsDefined) {
            this.Arm.DrawAll(r,g,b);
            for (i=0;i < this.numPoints;i++) {
                if(i == this.PointMemorized) this.Points[i].DrawSq(255-r,255-g,255-b);
                else                         this.Points[i].DrawSq(r,g,b);
            }
        }
        if(this.PointMemorized != -1) {
            this.ExtraPoint.DrawOX(171,10,224,50);
            this.ExtraPoint.Print("Extra");
        }
    }
    //------------------------------------------------------------------//
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class ArmSegmentChain {
    // Static Geometry
    float seg0_p0_distToEdge = 10;
    float seg0_p1_distToEdge = 10;
    float seg0_width = 20;
    float seg1_p0_distToEdge = 30;
    float seg1_p1_distToEdge = 10;
    float seg1_width = 60;
    float seg2_p0_distToEdge = 10;
    float seg2_p1_distToEdge = 120;
    float seg2_width = 30;
    // Arms
    int numPoints = 4;
    int numArmLines = 12;   // 3 Segments -> 3 Rects -> 12 Lines
    boolean ArmLinesDefined = false;
    Line  [] ArmLines;
    Point [] Points;
    float [] Angles;
    float [] ArmLengths;
    boolean MassageAngles = true;
    // Motor Control
    int MoveTowardsPPP = 0;
    boolean EnableArmUpdate = false;
    float [] MaxAngles = {180.,315.,200.};
    float [] MinAngles = {45.,45.,155.};
    int [] Motors = {0,0,0,0};
    int Duration = 0;
    float Damper = 3.5;
    int MagicDuration = 40;
    int MagicThreshold = 90;
    // Hand Motor Control
    boolean ChangingHandState = false;
    boolean ConstantPressureUpdate = false;
    int OnPressure = 5;
    int Pressure = 0;
    int Openness = 0;
    // Debug
    int DisplayStatus = 2;//3;
    //------------------------------------------------------------------//
    ArmSegmentChain() {
        this.ArmLines   = new Line[this.numArmLines];
        this.Points     = new Point[this.numPoints];
        this.Angles     = new float[this.numPoints-1];
        this.ArmLengths = new float[this.numPoints-1];
    }
    //------------------------------------------------------------------//
    void SetPoints(Point [] In, float HandLength) {
        for(int i=1;i<4;i++) {
            this.Points[i] = new Point(In[i].X - In[0].X + this.Points[0].X,
                                       In[i].Y - In[0].Y + this.Points[0].Y,
                                       In[i].Z);
        }
        this.seg2_p1_distToEdge = HandLength;
        this.BuildArmSegments();
    }
    //------------------------------------------------------------------//
    void InitPoints() {
        this.Points[0] = new Point(ScreenWidth*2./3.,ScreenHeight/**2./3.*/,20);
        this.Points[1] = new Point(this.Points[0].X,this.Points[0].Y-160,20);
        this.Points[2] = new Point(this.Points[0].X,this.Points[1].Y-167,20);
        this.Points[3] = new Point(this.Points[0].X,this.Points[2].Y-62,20);
        this.BuildArmSegments();
    }
    //------------------------------------------------------------------//
    void BuildArmSegments(){
        this.ArmLinesDefined = true;
        this.UpdateAngles();
        this.BuildArmSegment(0,this.Points[0],this.Points[1],this.seg0_p0_distToEdge,this.seg0_p1_distToEdge,this.seg0_width);
        this.BuildArmSegment(1,this.Points[1],this.Points[2],this.seg1_p0_distToEdge,this.seg1_p1_distToEdge,this.seg1_width);
        this.BuildArmSegment(2,this.Points[2],this.Points[3],this.seg2_p0_distToEdge,this.seg2_p1_distToEdge,this.seg2_width);
    }
    //------------------------------------------------------------------//
    void BuildArmSegment(int id, Point A, Point B, float A_distToEdge, float B_distToEdge, float Seg_width){
        // Make new 'origin'
        Point T = new Point(B.X-A.X,-(B.Y-A.Y)); // Note the -() is because: (height-B.Y-(height-A.Y))
        float length_AB = T.SqrtSumSq();
        float angle_AB = T.GetCurrentAngle();
        // Update CLASS internal vars:
        this.ArmLengths[id] = length_AB;
        // Build points as if angle where zero
        Point P0 = new Point(length_AB+B_distToEdge,Seg_width/2.0);
        Point P1 = new Point(-A_distToEdge,Seg_width/2.0);
        Point P2 = new Point(-A_distToEdge,-Seg_width/2.0);
        Point P3 = new Point(length_AB+B_distToEdge,-Seg_width/2.0);
        // Rotate points about 'origin' by angle
        P0.rotateCCWAboutOrigin(angle_AB);
        P1.rotateCCWAboutOrigin(angle_AB);
        P2.rotateCCWAboutOrigin(angle_AB);
        P3.rotateCCWAboutOrigin(angle_AB);
        // Return offset
        P0.AddOffset(A.X,A.Y);
        P1.AddOffset(A.X,A.Y);
        P2.AddOffset(A.X,A.Y);
        P3.AddOffset(A.X,A.Y);
        // Use points to make lines
        this.ArmLines[id*4+0] = new Line(P0,P1);
        this.ArmLines[id*4+1] = new Line(P1,P2);
        this.ArmLines[id*4+2] = new Line(P2,P3);
        this.ArmLines[id*4+3] = new Line(P3,P0);
    }
    //------------------------------------------------------------------//
    void UpdateAngles() {
        Point Orig = new Point(this.Points[0].X + 100, this.Points[0].Y);
        this.Angles[0] = this.RelativeAngle(Orig,this.Points[0],this.Points[1]);
        this.Angles[1] = this.RelativeAngle(this.Points[0],this.Points[1],this.Points[2]);
        this.Angles[2] = this.RelativeAngle(this.Points[1],this.Points[2],this.Points[3]);
    }
    //------------------------------------------------------------------//
    float RelativeAngle(Point A, Point B, Point C) {
        // Put B at the origin and translate A and C
        Point A_offset = new Point(A.X-B.X,A.Y-B.Y);
        Point C_offset = new Point(C.X-B.X,C.Y-B.Y);
        // Compute angle diff and return
        float AngleA = A_offset.GetCurrentAngle();
        float AngleC = C_offset.GetCurrentAngle();
        float temp = AngleA-AngleC;
        if(temp < 0) temp += 2.*PI;
        return temp*180./PI;
    }
    //------------------------------------------------------------------//
    void RunMotors() {
        String ToSend = "B_" + this.Motors[0] + "_" + this.Motors[1] + "_" + this.Motors[2] + "_" + this.Motors[3] + "_" + this.Duration + "_E";
        myPort.write(ToSend);
        if(this.DisplayStatus > 2) println("Serial Send: " + ToSend);
        delay(this.Duration);
    }
    //------------------------------------------------------------------//
    void CloseHand() {
        int NumBursts=0;
        String ToSend = "B_128_0_0_0_20_E";
        this.ChangingHandState = true;
        while((this.Pressure < this.OnPressure)&&(NumBursts < 80)) {
            myPort.write(ToSend);
            delay(40);
            NumBursts++;
        }
        println("Done CloseHand");
        this.ChangingHandState = false;
    }
    //------------------------------------------------------------------//
    void OpenHandSmallBit() {
        String ToSend = "B_-255_0_0_0_88_E";
        this.ChangingHandState = true;
        myPort.write(ToSend);
        delay(100);
        this.ChangingHandState = false;
    }
    //------------------------------------------------------------------//
    void OpenHand() {
        int NumBursts=0;
        String ToSend = "B_-255_0_0_0_50_E";
        this.ChangingHandState = true;
        do {
            myPort.write(ToSend);
            delay(100);
            NumBursts++;
        } while((this.Pressure > 0)&&(NumBursts < 5));

        NumBursts = 0;
        ToSend = "B_-128_0_0_0_30_E";
        while((this.Openness > 8)&&(NumBursts < 40)) {
            myPort.write(ToSend);
            delay(100);
            NumBursts++;
        }
        println("Done OpenHand");
        this.ChangingHandState = false;
    }
    //------------------------------------------------------------------//
    void MoveTowardsAngles(float [] FeedbackAngles, boolean [] Violators, int numAngles) {
        if(this.MoveTowardsPPP == 1) {
            float [] PreProgramedPosition = {90,180,180};
            RobotArm.MoveTowardsAngles(PreProgramedPosition,WiiRemote.Angles, WiiRemote.ViolatingSegments, 3);
        } else if(this.MoveTowardsPPP == 2) {
            float [] PreProgramedPosition = {51,312,164};
            RobotArm.MoveTowardsAngles(PreProgramedPosition,WiiRemote.Angles, WiiRemote.ViolatingSegments, 3);
        } else if(this.MoveTowardsPPP == 3) {
            float [] PreProgramedPosition = {126,53,185};
            RobotArm.MoveTowardsAngles(PreProgramedPosition,WiiRemote.Angles, WiiRemote.ViolatingSegments, 3);
        } else if(this.MoveTowardsPPP == 4) {
            float [] PreProgramedPosition = {156,207,161};
            RobotArm.MoveTowardsAngles(PreProgramedPosition,WiiRemote.Angles, WiiRemote.ViolatingSegments, 3);
        } else {
            this.MoveTowardsAngles(this.Angles,FeedbackAngles,Violators,numAngles);
        }
    }
    void MoveTowardsAngles(float [] DestAngles, float [] FeedbackAngles, boolean [] Violators, int numAngles) {
        boolean RunMotorsIsWorthIt = false;
        float [] DeltaAngles = {0,0,0};
        int MaxDeltaIndex = 0;
        for(int i=0;i < numAngles;i++) {
            // Get delta angle in and clip to static limits
            float DestAngleClipped = DestAngles[i];
            if(DestAngleClipped > this.MaxAngles[i]) DestAngleClipped = this.MaxAngles[i];
            if(DestAngleClipped < this.MinAngles[i]) DestAngleClipped = this.MinAngles[i];
            DeltaAngles[i] = DestAngleClipped - FeedbackAngles[i];
            if(DeltaAngles[i] > DeltaAngles[MaxDeltaIndex]) MaxDeltaIndex = i;
        }
        for(int i=0;i < numAngles;i++) {
            // Angle->Motor function
            this.Motors[i+1] = (int)(DeltaAngles[i]*255./this.Damper);
            // Clip Motor speeds
            if(this.Motors[i+1] >  255) this.Motors[i+1] =  255;
            if(this.Motors[i+1] < -255) this.Motors[i+1] = -255;
        }
        // Now to apply violation correction
        boolean Violation = false;
        for(int i=numAngles-1;i>=0;i--) {
            Violation |= Violators[i];
            if(Violation) this.Motors[i+1] *= -0.5;
        }
        // Determin if it's all even worth it
        for(int i=0;i < numAngles;i++) {
            if(abs(this.Motors[i+1]) > this.MagicThreshold) RunMotorsIsWorthIt = true;
        }
        RobotArm.Motors[0] = 0; // Only used in Hand Open/Close
        RobotArm.Duration = this.MagicDuration;
        if(RunMotorsIsWorthIt) RobotArm.RunMotors();
    }
    //------------------------------------------------------------------//
    void AlignHandToPoint(Point ExtraIn, Point Origin) {
        // Copy the point and re-offset to be relative to this arm's Origin (i.e. Points[0])
        Point AdjustedIn = new Point(ExtraIn.X,ExtraIn.Y,ExtraIn.Z);
        AdjustedIn.SubtractOffset(Origin);
        // AdjustedIn.rotateCWAboutOrigin(10./180*PI);
        boolean LeftSide = (AdjustedIn.X>0);
        float FullDist = AdjustedIn.SqrtSumSq();
        AdjustedIn.AddOffset(this.Points[0]);
        if(FullDist > (this.seg2_p1_distToEdge+this.ArmLengths[0]+this.ArmLengths[1]+this.ArmLengths[2])) {
            this.Points[1] = this.Points[0].FindPointAtDistInDirOfAnother(this.ArmLengths[0],AdjustedIn);
            this.Points[2] = this.Points[1].FindPointAtDistInDirOfAnother(this.ArmLengths[1],AdjustedIn);
            this.Points[3] = this.Points[2].FindPointAtDistInDirOfAnother(this.ArmLengths[2],AdjustedIn);
            this.BuildArmSegments();
        } else {
            // Place new point3 this.seg2_p1_distToEdge (in the direction of point 2) away from AdjustedIn
            Point NewP3 = AdjustedIn.FindPointAtDistInDirOfAnother(this.seg2_p1_distToEdge,this.Points[2]);
            // Place new point2 this.ArmLengths[2] away from new point3 (in the direction of Point2)
            Point NewP2 = NewP3.FindPointAtDistInDirOfAnother(this.ArmLengths[2], this.Points[2]);
            // Solve for the last point (because the base can't move, Points[0] will be unchanged)
            Point NewP1 = this.SolveForPoint(this.Points[0],this.ArmLengths[0],NewP2,this.ArmLengths[1],LeftSide);
            if(NewP1.Z != -1) { // Then was solvable
                // Now to update the points
                this.Points[3] = new Point(NewP3.X,NewP3.Y,this.Points[3].Z);
                this.Points[2] = new Point(NewP2.X,NewP2.Y,this.Points[2].Z);
                this.Points[1] = new Point(NewP1.X,NewP1.Y,this.Points[1].Z);
                if(MassageAngles) this.MassageAnglesIfInvalid(AdjustedIn);
                this.BuildArmSegments();
            }
        }
    }
    //------------------------------------------------------------------//
    Point SolveForPoint(Point Origin, float DistToOrig, Point C, float DistToC, boolean LeftSide){
        Point ToReturn = new Point(0,0,-1);
        // boolean LeftSide = (C.X>Origin.X);
        float DistCToOrigin = C.RelativeDist(Origin);
        if(DistCToOrigin <= (DistToC+DistToOrig)) { // Then solution exists
            // Get Point2's angle relative to Point0 and the axis there
            Point temp = new Point(Origin.X + 100, Origin.Y);
            float AngleToPoint2 = this.RelativeAngle(temp,Origin,C)*PI/180.;
            // Get the angle of the triangle (P0.P1.P2) at P2
            float AngleForPoint2 = acos(((DistToC*DistToC)+(DistCToOrigin*DistCToOrigin)-(DistToOrig*DistToOrig)) / (2.*DistToC*DistCToOrigin));
            // Place P1 on x-axis and rotate CW to build X,Y values (relative to P2)
            ToReturn = new Point(DistToC, 0.0, 0);
            float rotateAngle = (PI-AngleToPoint2-AngleForPoint2);
            ToReturn.rotateCWAboutOrigin(rotateAngle); // Note the 'origin' is C
            if(LeftSide) ToReturn.rotateCWAboutOrigin(2*AngleForPoint2);
            // Offset back to where it should be
            ToReturn.AddOffset(C);
        } else if(this.DisplayStatus > 2){
            println("Impossibly grabbable Extra point! " + DistCToOrigin + " >= " + (DistToC+DistToOrig));
        }
        return ToReturn;
    }
    //------------------------------------------------------------------//
     void MassageAnglesIfInvalid(Point Target) {
        float TrickleAngle = 0;
        boolean LeftSide = (this.Points[2].X>this.Points[0].X);
        this.UpdateAngles();
        TrickleAngle = (this.Angles[2] > this.MaxAngles[2])? this.Angles[2]-this.MaxAngles[2] :
                       (this.Angles[2] < this.MinAngles[2])? this.Angles[2]-this.MinAngles[2] : 0;
        TrickleAngle /= 16.;
        if(TrickleAngle != 0) {
            // Rotate P3 and P2 about Target by TrickleAngle
            Point NewP3 = new Point(this.Points[3].X,this.Points[3].Y);
            NewP3.rotateCWAboutRefOrigin(TrickleAngle,Target);
            Point NewP2 = new Point(this.Points[2].X,this.Points[2].Y);
            NewP2.rotateCWAboutRefOrigin(TrickleAngle,Target);
            Point NewP1 = this.SolveForPoint(this.Points[0],this.ArmLengths[0],NewP2,this.ArmLengths[1],LeftSide);
            if(NewP1.Z != -1) { // Then was solvable
                // Now to test for Delta angles past realistic
                float maxDeltaAngle = 45; // No jumping to the other side
                Point Orig = new Point(this.Points[0].X + 100, this.Points[0].Y);
                float Angle0 = this.RelativeAngle(Orig,this.Points[0],NewP1);
                float Angle1 = this.RelativeAngle(this.Points[0],NewP1,NewP2);
                float Angle2 = this.RelativeAngle(NewP1,NewP2,NewP3);
                if((abs(Angle0-this.Angles[0])= 1) this.DebugInfo(r,g,b);
    }
    //------------------------------------------------------------------//
    // DEBUG
    void DebugInfo(int r, int g, int b) {
        stroke(r,g,b);
        fill  (r,g,b);
        // Angles
        for (int i=0;i < this.numPoints-1;i++) text("      Angle" + i + ":" + this.Angles[i], this.Points[i].X,this.Points[i].Y);
        if(this.DisplayStatus > 1) {
            // Motors:
            text("M0(" + this.Motors[0] + "), M1(" + this.Motors[1] + "), M2(" + this.Motors[2] + "), M3(" + this.Motors[3] + "), Duration(" + this.Duration + ")", 0, height-20);
            // Settings:
            text("EnableArmUpdate:" + this.EnableArmUpdate, 0, height-40);
            text("Pressure:" + this.Pressure + "  Openness:" + this.Openness, 0, height-60);
            text("ConstantPressureUpdate:" + this.ConstantPressureUpdate, 0, height-80);
            text("MoveTowardsPreprogramedPosition:" + this.MoveTowardsPPP, 0, height-100);
        }
        if(this.DisplayStatus > 2) {
            text("Damper:" + this.Damper + "  MagicThreshold:" + this.MagicThreshold + "  MagicDuration:" + this.MagicDuration, 0, height-120);
            // Lines and Points
            for (int i=0;i < this.numPoints-1;i++) {
                text("   Line" + i + ":" + (int)this.ArmLengths[i],
                    (this.Points[i].X+this.Points[i+1].X)/2.0,
                    (this.Points[i].Y+this.Points[i+1].Y)/2.0);
                this.Points[i].PrintID(i);
                this.Points[i].DrawSq(r,g,b);
            }
            this.Points[this.numPoints-1].PrintID(this.numPoints-1);
            this.Points[this.numPoints-1].DrawSq(r,g,b);
        }
    }
    //------------------------------------------------------------------//
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class ConstraintsArrayList extends ArrayList {
    int ErrorOn = 0;
    //------------------------------------------------------------------//
    Line Pop(int index) {
        if(this.size()>index) {
            Line ToReturn;
            ToReturn = (Line)this.get(index);
            this.remove(index);
            return ToReturn;
        } else {
            return (Line)null;
        }
    }
    //------------------------------------------------------------------//
    int TestForIntersection(Line In) {
        ErrorOn = 0; // Assume pass
        for(int i=0;i < this.size();i++) {
            if(In.Intersects((Line)this.get(i))) {
                ErrorOn++;
                break;
            }
        }
        return ErrorOn;
    }
    int TestForIntersection(Line[] In, int size) {
        ErrorOn = 0; // Assume pass
        for(int i=0;i < this.size();i++) {
            for(int j=0;j < size;j++) {
                if(In[j].Intersects((Line)this.get(i))) {
                    ErrorOn++;
                    break;
                }
            }
        }
        return ErrorOn;
    }
    //------------------------------------------------------------------//
    void DrawAll(int r, int g, int b) {
        for(int i=0;i < this.size();i++) ((Line)this.get(i)).DrawLine(r,g,b);
        if(ErrorOn > 0) {
            Point ERROR = new Point(width/2,height/2);
            ERROR.DrawX( r,g,b,100);
            ERROR.DrawSq(r,g,b,50);
        }
    }
    //------------------------------------------------------------------//
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class Line {
    Point A,B;
    //------------------------------------------------------------------//
    Line(Point a, Point b) {
        this.A = new Point(a.X,a.Y);
        this.B = new Point(b.X,b.Y);
    }
    //------------------------------------------------------------------//
    float LineLength() {
        return sqrt((this.A.X-this.B.X)*(this.A.X-this.B.X)+(this.A.Y-this.B.Y)*(this.A.Y-this.B.Y));
    }
    //------------------------------------------------------------------//
    boolean Intersects(Line In) {
        return ((this.DeltaSlopeTest(In.A)^this.DeltaSlopeTest(In.B))&&(In.DeltaSlopeTest(this.A)^In.DeltaSlopeTest(this.B)));
    }
    //------------------------------------------------------------------//
    boolean DeltaSlopeTest(Point In) {
        return (((In.Y-A.Y)*(B.X-A.X))>((B.Y-A.Y)*(In.X-A.X)));
    }
    //------------------------------------------------------------------//
    void DrawLine(int r, int g, int b) {
        stroke(r,g,b);
        fill  (r,g,b);
        line(this.A.X,this.A.Y,this.B.X,this.B.Y);
    }
    //------------------------------------------------------------------//
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class Point {
    float X,Y,Z; // Z is used for points' intensity
    //------------------------------------------------------------------//
    Point(float x, float y) {
        this.X = x;
        this.Y = y;
        this.Z = 0;
    }
    Point(float x, float y, float z) {
        this.X = x;
        this.Y = y;
        this.Z = z;
    }
    //------------------------------------------------------------------//
    void rotateCWAboutOrigin(float Angle) {
        this.rotateCCWAboutOrigin((2*PI)-Angle);
    }
    //------------------------------------------------------------------//
    void rotateCCWAboutOrigin(float Angle) {
        float tempLength = this.SqrtSumSq();
        float currentAngle = this.GetCurrentAngle();
        float NewX = tempLength*cos(currentAngle-Angle);
        float NewY = tempLength*sin(currentAngle-Angle);
        this.X = NewX;
        this.Y = NewY;
    }
    //------------------------------------------------------------------//
    void rotateCWAboutRefOrigin(float Angle, Point RefOrigin) {
        this.rotateCCWAboutRefOrigin((2*PI)-Angle, RefOrigin);
    }
    //------------------------------------------------------------------//
    void rotateCCWAboutRefOrigin(float Angle, Point RefOrigin) {
        this.SubtractOffset(RefOrigin);
        this.rotateCCWAboutOrigin(Angle);
        this.AddOffset(RefOrigin);
    }
    //------------------------------------------------------------------//
    void AddOffset(Point Offset) {
        this.X += Offset.X;
        this.Y += Offset.Y;
    }
    void AddOffset(float X_offset, float Y_offset) {
        this.X += X_offset;
        this.Y += Y_offset;
    }
    //------------------------------------------------------------------//
    void SubtractOffset(Point Offset) {
        this.X -= Offset.X;
        this.Y -= Offset.Y;
    }
    void SubtractOffset(float X_offset, float Y_offset) {
        this.X -= X_offset;
        this.Y -= Y_offset;
    }
    //------------------------------------------------------------------//
    float SqrtSumSq() {
        return sqrt((this.X*this.X)+(this.Y*this.Y));
    }
    //------------------------------------------------------------------//
    float RelativeDist(Point Ref) {
        return sqrt(((this.X-Ref.X)*(this.X-Ref.X))+((this.Y-Ref.Y)*(this.Y-Ref.Y)));
    }
    //------------------------------------------------------------------//
    float GetCurrentAngle() {
        float result = abs(atan(this.Y/this.X));
        if ((this.X < 0)&&(this.Y < 0)) {
            result += PI;
        } else if (this.X < 0) {
            result = PI - result;
        } else if (this.Y < 0) {
            result = -result;
        }
        return result;
    }
    //------------------------------------------------------------------//
    Point FindPointAtDistInDirOfAnother(float Distance, Point Direction) {
        float SimilarTriangleRatio = Distance / this.RelativeDist(Direction);
        Point ToReturn = new Point(SimilarTriangleRatio*(Direction.X-this.X)+this.X,SimilarTriangleRatio*(Direction.Y-this.Y)+this.Y);
        return ToReturn;
    }
    //------------------------------------------------------------------//
    void DrawX(int r, int g, int b) {
        stroke(r,g,b);
        line(this.X-this.Z/2,this.Y-this.Z/2,this.X+this.Z/2,this.Y+this.Z/2);
        line(this.X-this.Z/2,this.Y+this.Z/2,this.X+this.Z/2,this.Y-this.Z/2);
    }
    void DrawX(int r, int g, int b, float Height) {
        stroke(r,g,b);
        Height/=2; // Scale once in begining is more efficient, but I guess no scale is better :)
        line(this.X-Height,this.Y-Height,this.X+Height,this.Y+Height);
        line(this.X-Height,this.Y+Height,this.X+Height,this.Y-Height);
    }
    //------------------------------------------------------------------//
    void DrawO(int r, int g, int b) {
        stroke(r,g,b);
        fill  (r,g,b);
        ellipse(this.X,this.Y,this.Z,this.Z);
    }
    void DrawO(int r, int g, int b, float Radius) {
        stroke(r,g,b);
        fill  (r,g,b);
        ellipse(this.X,this.Y,Radius,Radius);
    }
    //------------------------------------------------------------------//
    void DrawOX(int r, int g, int b) {
        stroke(r,g,b);
        noFill();
        ellipse(this.X,this.Y,this.Z,this.Z);
        this.DrawX(r,g,b);
    }
    void DrawOX(int r, int g, int b, float Radius) {
        stroke(r,g,b);
        noFill();
        ellipse(this.X,this.Y,Radius,Radius);
        this.DrawX(r,g,b,Radius);
    }
    //------------------------------------------------------------------//
    void DrawSq(int r, int g, int b) {
        stroke(r,g,b);
        fill  (r,g,b);
        rect(this.X-this.Z/4,this.Y-this.Z/4,this.Z/2,this.Z/2);
    }
    void DrawSq(int r, int g, int b, float Height) {
        stroke(r,g,b);
        fill  (r,g,b);
        Height/=2; // Scale once in begining is more efficient, but I guess no scale is better :)
        rect(this.X-Height/2,this.Y-Height/2,Height,Height);
    }
    //------------------------------------------------------------------//
    void PrintID(int ID) {
        text("ID:" + ID, this.X-20,this.Y-20);
    }
    //------------------------------------------------------------------//
    void Print(String InStr) {
        text(InStr, this.X-InStr.length()*6,this.Y-20);
    }
    //------------------------------------------------------------------//
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////