HND: Weight Shifting

Weight shifting sphere using 2 servo motors and a large weight. One servo is for z rotation, and another is for the x rotation ( or degree of weight shift ).

HND: Weight Shifting 02 from che-wei wang on Vimeo.

HND : Weight Shifting 01 from che-wei wang on Vimeo.

Processing code to control weight shifting behavior via mouse:
Calculates atan from center of the screen for angle and distance to center of the screen for weight shift.
On screen mouse control to be replaced by GPS coordinate distance and angle later.

import processing.serial.*;

PFont font;
int [] serialOut = new int [2];
int [] serialIn = new int [2];
boolean handshake=false;
Serial port;

int  distance,angle;

void setup(){
  size(800,800);
  noCursor();
  font = createFont("FFScala", 14);
  println(Serial.list());
  port = new Serial(this, Serial.list()[0], 9600);
  port.write(255);
}

void draw(){
  background(150);
  noFill();

  if (handshake == true){
    float diam=width*.75;
    stroke(160);
    fill(160);
    ellipse(width/2,height/2,diam,diam);
    stroke(255);
    line(width/2,height/2,mouseX,mouseY);
    //north
    line(width/2-diam/4,height/2,width/2-diam/2,height/2);
    //position
    fill(10);
    noStroke();
    ellipse(mouseX,mouseY,30,30);

    float a = atan2(mouseY-height/2, mouseX-width/2)/TWO_PI; //float from 0-1
    angle=int((a)*254+128);//translate angle to 0-255

    float d1=dist(mouseX,mouseY,height/2,width/2)/diam*2;// float from 0-1
    distance=int(d1*128);//translate distance to 0-128

//simlulate 360 rotation with 180 servo rotation
    if(angle<128){
      distance=distance+128;
      angle=255-angle*2;
    }
    else{
      distance=128-distance;
      angle=255-(angle-128)*2;
    }

    serialOut[0]=(constrain(distance,0,254));//constrain output to 254, using 255 as header
    serialOut[1]=(constrain(angle,0,254));

    port.write(255); //header
    port.write(serialOut[0]);
    port.write(serialOut[1]);

    fill(200);
    textFont(font, 14);
    text(serialOut[0], mouseX+20, mouseY);
    text(serialOut[1], height/2+20, width/2);
  }
}

void serialEvent(Serial port) {
  if (handshake == false) {
    handshake = true;
  }
  int inByte=port.read();
}

Arduino code:
Interprets angle and distance values from Processing to pulses for servo motor rotation angles

int minPulse = 500;   // Minimum servo position
int maxPulse = 2500;  // Maximum servo position
int pulse,pulseR;

long lastPulse = 0;    // the time in milliseconds of the last pulse
int refreshTime = 20; // the time needed in between pulses

int distance=0;
int angle=0;

void setup() {
  pinMode(11, OUTPUT);  // Set servo pin as an output pin
  pinMode(10, OUTPUT);
  pulse = minPulse;           // Set the motor position value to the minimum
  pulseR = minPulse;
  Serial.begin(9600);
  Serial.print(65);
}

void loop() {

  if (Serial.available()>=3) {
    int serialIn=Serial.read();
    if(serialIn==255){
      distance=Serial.read();
      angle=Serial.read();
    }
  }

  pulse = distance*4*19/10+minPulse;    // convert the analog value
  pulseR = angle*4*19/10+minPulse;    // convert the analog value

    // pulse the servo again if rhe refresh time (20 ms) have passed:
  if (millis() - lastPulse >= refreshTime) {
    digitalWrite(11, HIGH);   // Turn the motor on
    delayMicroseconds(pulseR);       // Length of the pulse sets the motor position
    digitalWrite(11, LOW);    // Turn the motor off
    digitalWrite(10, HIGH);   // Turn the motor on
    delayMicroseconds(pulse);       // Length of the pulse sets the motor position
    digitalWrite(10, LOW);    // Turn the motor off 

    lastPulse = millis();           // save the time of the last pulse
  }

  Serial.print(255,BYTE); //send something
}