During the debugging process I wrote an app in Processing that simulates the movement of the entire compass mechanism and outputs serial data to control the motors.
Here's what it looks like on the actual physical mechanism:
The complete code is up on github. The app works by summing combinations of primary, secondary, and tertiary rotations of the mechanism to achieve interesting movements. Then it uses the gear ratios of my models to calculate the speeds of the four driving motors and send this data out to the electronics.
I used a few external libraries for this app:
Modelbuilder - by Marius Watz is a library that lets you import and export STLs from Processing. I recommend checking it out if you're interested in making generative 3d models. I used Modelbuilder to import and rotate my models. Modelbuilder stores STL geometry in a UGeometry object and has functions rotateX, rotateY, and rotateZ that work similarly to the Processing functions. I used an older version of modelbuild found here (v0007a01.zip).
import unlekker.util.*;
import unlekker.modelbuilder.*;
//models
String[] allFiles = {
"stls/discTertGear.stl",
"stls/tertGear.stl",
"stls/secondaryGear1.stl",
"stls/secondaryGear2.stl",
"stls/primaryGear1.stl",
"stls/primaryGear2.stl",
"stls/primaryGear1.stl",
"stls/primaryGear2.stl",
"stls/support1.stl",
"stls/support2.stl"};
RotatingBody[] allBodies = new RotatingBody[allFiles.length];
I had to do something strange to get my rotations to work out - the rotateX, rotateY, and rotateZ methods only do relative rotations of the model, and there are no methods for doing absolute rotations. There are also no methods for resetting the model back to its unrotated position. I put each of my models in an instance of the RotatingBody class so that I could store the current absolute rotation of the model in x, y, and z. In each draw() cycle I undid the current rotations to get back to my initial load orientation, then I added my new rotations to the saved rotations, and then I rotated the model with rotateX, Y and Z.
class RotatingBody {
UGeometry model;
PApplet context;
PVector[] rotations = new PVector[3];
RotatingBody(PApplet context, String filename){
context = context;
model = importFile(context, filename);
for (int i=0;i<rotations.length; i++){
rotations[i] = new PVector(0, 0, 0);
}
}
UGeometry importFile(PApplet context, String file){
UGeometry model = new UGeometry();
model = UGeometry.readSTL(context,file);
model.scale(10.0);
return model;
}
void rotateX(float angle, int degree){
(rotations[degree]).x += angle;
}
void rotateY(float angle, int degree){
(rotations[degree]).y += angle;
}
void rotateZ(float angle, int degree){
(rotations[degree]).z += angle;
}
void undoRotations(){
//start with primary rotations first
for (PVector rotation : rotations){
model.rotateX(-rotation.x);
model.rotateY(-rotation.y);
model.rotateZ(-rotation.z);
}
}
void doRotations(){
//start with tertiary rotations first
for (int i = rotations.length-1; i>=0; i--){
PVector rotation = rotations[i];
model.rotateX(rotation.x);
model.rotateY(rotation.y);
model.rotateZ(rotation.z);
}
}
void draw(PApplet context){
model.draw(context);
}
}PeasyCam - by Jonathan Feinberg is a library that allows you to easily control the movement of the camera in 3D with a virtual trackball. I'm using it to rotate the model around the Z axis.
import peasy.*;
PeasyCam cam;
void setupCam(){
cam = new PeasyCam(this, 50);
cam.setMinimumDistance(200);
cam.setMaximumDistance(200);
cam.setYawRotationMode();
cam.pan(0,-20);
}
ControlP5 - by Andreas Schlegel is a framework for setting up UI elements in your 2D or 3D processing sketch. It works nicely with PeasyCam.
import controlP5.*;
//UI
ControlP5 cp5;
color highlightColor = color(255, 15, 0);
color secondaryColor = color(200);
void setupUI(){
int column1Offset = 50;
int column2Offset = 650;
int sliderWidth = 350;
cp5.addSlider("PRIMARY ROTATION").setPosition(column1Offset,20).setSize(sliderWidth,15).setRange(-100,100).setId(1).setColorCaptionLabel(10)
.setColorValue(color(255)).setColorActive(highlightColor).setColorForeground(highlightColor).setColorBackground(secondaryColor).setSliderMode(Slider.FLEXIBLE);
cp5.addSlider("SECONDARY ROTATION").setPosition(column1Offset,50).setSize(sliderWidth,15).setRange(-100,100).setId(2).setColorCaptionLabel(10)
.setColorValue(color(255)).setColorActive(highlightColor).setColorForeground(highlightColor).setColorBackground(secondaryColor).setSliderMode(Slider.FLEXIBLE);
cp5.addSlider("TERTIARY ROTATION").setPosition(column1Offset,80).setSize(sliderWidth,15).setRange(-100,100).setId(3).setColorCaptionLabel(10)
.setColorValue(color(255)).setColorActive(highlightColor).setColorForeground(highlightColor).setColorBackground(secondaryColor).setSliderMode(Slider.FLEXIBLE);
cp5.addButton("STOP").setValue(0).setPosition(column1Offset,110).setSize(35,19).setColorActive(secondaryColor).setColorForeground(color(100)).setColorBackground(highlightColor);
cp5.addSlider("MOTOR 1").setPosition(column2Offset,20).setSize(sliderWidth,15).setRange(-100,100).setId(4).setColorCaptionLabel(10)
.setColorValue(color(255)).setColorActive(highlightColor).setColorForeground(highlightColor).setColorBackground(secondaryColor).setLock(true).setSliderMode(Slider.FLEXIBLE);
cp5.addSlider("MOTOR 2").setPosition(column2Offset,50).setSize(sliderWidth,15).setRange(-100,100).setId(5).setColorCaptionLabel(10)
.setColorValue(color(255)).setColorActive(highlightColor).setColorForeground(highlightColor).setColorBackground(secondaryColor).setLock(true).setSliderMode(Slider.FLEXIBLE);
cp5.addSlider("MOTOR 3").setPosition(column2Offset,80).setSize(sliderWidth,15).setRange(-100,100).setId(6).setColorCaptionLabel(10)
.setColorValue(color(255)).setColorActive(highlightColor).setColorForeground(highlightColor).setColorBackground(secondaryColor).setLock(true).setSliderMode(Slider.FLEXIBLE);
cp5.addSlider("MOTOR 4").setPosition(column2Offset,110).setSize(sliderWidth,15).setRange(-100,100).setId(7).setColorCaptionLabel(10)
.setColorValue(color(255)).setColorActive(highlightColor).setColorForeground(highlightColor).setColorBackground(secondaryColor).setLock(true).setSliderMode(Slider.FLEXIBLE);
}
The event handlers for my UI:
void controlEvent(ControlEvent event) {
switch(event.getController().getId()){
case 1:
primaryRotRate = event.getController().getValue()/1000.0;
break;
case 2:
secondaryRotRate = event.getController().getValue()/1000.0;
break;
case 3:
tertiaryRotRate = event.getController().getValue()/1000.0;
break;
}
if (event.getController().getId()<=3){
calculateMotorRotations();
}
}
//button events
public void STOP() {
primaryRotRate = 0.0;
secondaryRotRate = 0.0;
tertiaryRotRate = 0.0;
silentUpdateAxisSliders();
motor1Rate = 0.0;
motor2Rate = 0.0;
motor3Rate = 0.0;
motor4Rate = 0.0;
silentUpdateMotorSliders();
}
I added a bit of code to draw the flat elements in my 3D environment so that they do not rotate with PeasyCam:
void drawGui() {
hint(DISABLE_DEPTH_TEST);
cam.beginHUD();
makeGrad();
noLights();
cp5.draw();
cam.endHUD();
hint(ENABLE_DEPTH_TEST);
}
And one more piece to disable PeasyCam motions when dragging the controlP5 elements:
void disablePeasyP5Clash(){
if (mouseY<150) {
cam.setActive(false);
} else {
cam.setActive(true);
}
}I also used the Serial library that comes with Processing to output data to the Atmega chip controlling my motors.
//send out serial port.write(scaleMotorOutputVal(motor1Rate)+binaryPhaseOutput(motor1Rate)); port.write(scaleMotorOutputVal(motor2Rate)+binaryPhaseOutput(motor2Rate)+64); port.write(scaleMotorOutputVal(motor3Rate)+binaryPhaseOutput(motor3Rate)+128); port.write(scaleMotorOutputVal(motor4Rate)+binaryPhaseOutput(motor4Rate)+192);
For now, messages are sent as bytes where the first two bits are the address of the motor, the next bit is the direction, and the last five bits are the speed:
AADSSSSS
The code running on my board parses incoming bytes and sends the data to the correct motor:
int phase1 = A3;
int enable1 = A2;
byte motor1PWM = 0;
int phase2 = A1;
int enable2 = A0;
byte motor2PWM = 0;
int phase3 = 4;
int enable3 = A5;
byte motor3PWM = 0;
int phase4 = 8;
int enable4 = 7;
byte motor4PWM = 0;
byte currentCount = 0;
void setup(){
pinMode(phase1, OUTPUT);
pinMode(enable1, OUTPUT);
pinMode(phase2, OUTPUT);
pinMode(enable2, OUTPUT);
pinMode(phase3, OUTPUT);
pinMode(enable3, OUTPUT);
pinMode(phase4, OUTPUT);
pinMode(enable4, OUTPUT);
Serial.begin(9600);
cli();
//set timer0 interrupt at 2kHz
TCCR0A = 0;// set entire TCCR0A register to 0
TCCR0B = 0;// same for TCCR0B
TCNT0 = 0;//initialize counter value to 0
// set compare match register for 2khz increments
OCR0A = 124;// = (16*10^6) / (2000*64) - 1 (must be <256)
// turn on CTC mode
TCCR0A |= (1 << WGM01);
// Set CS01 and CS00 bits for 64 prescaler
TCCR0B |= (1 << CS01) | (1 << CS00);
// enable timer compare interrupt
TIMSK0 |= (1 << OCIE0A);
sei();//allow interrupts
}
void loop(){
if (Serial.available()){
byte data = Serial.read();
byte motorNum = (data>>6)&3;
boolean phase = (data>>5)&1;
byte val = data&31;
switch (motorNum){
case 0:
motor1PWM = val;
digitalWrite(phase1, phase);
break;
case 1:
motor2PWM = val;
digitalWrite(phase2, phase);
break;
case 2:
motor3PWM = val;
digitalWrite(phase3, !phase);
break;
case 3:
motor4PWM = val;
digitalWrite(phase4, !phase);
break;
}
}
}
ISR(TIMER0_COMPA_vect){//timer0 interrupt 2kHz
if (currentCount>=motor1PWM){
digitalWrite(enable1, LOW);
} else {
digitalWrite(enable1, HIGH);
}
if (currentCount>=motor2PWM){
digitalWrite(enable2, LOW);
} else {
digitalWrite(enable2, HIGH);
}
if (currentCount>=motor3PWM){
digitalWrite(enable3, LOW);
} else {
digitalWrite(enable3, HIGH);
}
if (currentCount>=motor4PWM){
digitalWrite(enable4, LOW);
} else {
digitalWrite(enable4, HIGH);
}
currentCount++;
if (currentCount>31) currentCount = 0;
}