球面プロッタ完成 (SPG20-332 + MP4401 その3)

Arduinoでステッピングモータ SPG20-332を回すやつの続きの続き。
Arduino側の制御プログラムが完成し、残りはベクターデータから(相対)座標を取り出してArduinoにコマンドを送るプログラムの制作のみ。

とりあえず、簡単な図形ということで、星型を描かせてみました。



始点と終点がずれているのは、ボールの回転軸が微妙にずれているため。
工作精度 or 強度の問題なので、ボールをしっかりと軸の中心に固定できるように固定具を作り直す必要がありそうです。

プログラムはこんな感じ。
鬼のようなネストなど存在しない。(エレガントに書き直す気力が無いので放置)

#include <Servo.h>

#define SERVO_PIN 3 //サーボモータの接続ピン
#define PEN_UP_ANGLE 115 //ペン上げ角度
#define PEN_DOWN_ANGLE 100 //ペン下げ角度
#define MOTOR_DELAY 7 //励磁間隔

const byte motorPen[]={
  0x09,0x0C,0x06,0x03}; //ペン軸 励磁パターン
const byte motorBall[]={
  0x30,0x60,0xC0,0x90}; //ボール軸 励磁パターン
Servo servoPen; //ペン持ち上げ用サーボを定義
char incoming[100]; //シリアルからのコマンド受け取り用
int x,y; //移動先座標と差分
int magDrivePen,magDriveBall; //現在の励磁状態を記憶

void setup(){
  DDRB |=B00001111; //ポートB出力設定
  DDRD |=B11110000; //ポートD出力設定
  servoPen.attach(SERVO_PIN); //サーボ出力ピン設定
  servoPen.write(PEN_UP_ANGLE); //ペン持ち上げ
  Serial.begin(9600); //シリアル通信9600bps
}

void getCommand(char *buf) //シリアル通信から座標を取得
{
  int i = 0;
  char c;
  while(1){
    if(Serial.available()){
      c = Serial.read();
      buf[i] = c;
      if (c == '\n') break;
      i++;
    }
  }
  buf[i] = '\0';
}

void movePenForward() //ペン前進
{
  magDrivePen++;
  if(magDrivePen>3)magDrivePen=0;
  PORTB &=0xf0;
  PORTB |= motorPen[magDrivePen];
  delay(MOTOR_DELAY);
}

void movePenBackward() //ペン後進
{
  magDrivePen--;
  if(magDrivePen<0)magDrivePen=3;
  PORTB &=0xf0;
  PORTB |= motorPen[magDrivePen];
  delay(MOTOR_DELAY); 
}

void moveBallForward() //ボール前進
{
  magDriveBall++;
  if(magDriveBall>3)magDriveBall=0;
  PORTD &=0x0f;
  PORTD |= motorBall[magDriveBall];
  delay(MOTOR_DELAY);
}

void moveBallBackward() //ボール後進
{
  magDriveBall--;
  if(magDriveBall<0)magDriveBall=3;
  PORTD &=0x0f;
  PORTD |= motorBall[magDriveBall];
  delay(MOTOR_DELAY); 
}

void writing(int dx, int dy) //座標移動(Bresenhamの線描アルゴリズム)
{
  int e = 0; //誤差蓄積用
  if(abs(dx) > abs(dy)){ //傾きが1未満の場合
    if(dx>=0){ //x軸方向が正の場合
      for (int sx = 0; sx < dx; sx++) {
        movePenForward();
        if(dy>=0){ //y軸方向が正の場合
          e += dy;
          if (e > dx){
            e -= dx;
            moveBallForward();
          }
        }
        else{ //y軸方向が負の場合
          e += abs(dy);
          if (e > dx){
            e -= dx;
            moveBallBackward();
          }
        }
      }
    }
    else{ //x軸方向が負の場合
      for (int sx = 0; sx < abs(dx); sx++) {
        movePenBackward();
        if(dy>=0){ //y軸方向が正の場合
          e += dy;
          if (e > abs(dx)){
            e -= abs(dx);
            moveBallForward();
          }
        }
        else{ //y軸方向が負の場合
          e += abs(dy);
          if (e > abs(dx)){
            e -= abs(dx);
            moveBallBackward();
          }
        }
      }
    }
  }
  else{ //傾きが1以上の場合
    if(dy>=0){ //y軸方向が正の場合
      for (int sx,sy = 0; sy < dy; sy++) {
        moveBallForward();
        if(dx>=0){ //x軸方向が正の場合
          e += dx;
          if (e > dy){
            e -= dy;
            movePenForward();
            sx++;
          }
        }
        else{ //x軸方向が負の場合
          e += abs(dx);
          if (e > dy){
            e -= dy;
            movePenBackward();
            sx++;
          }
        }
      }
    }
    else{ //y軸方向が負の場合
      for (int sx,sy = 0; sy < abs(dy); sy++) {
        moveBallBackward();
        if(dx>=0){ //x軸方向が正の場合
          e += dx;
          if (e > abs(dy)){
            e -= abs(dy);
            movePenForward();
            sx++;
          }
        }
        else{ //x軸方向が負の場合
          e += abs(dx);
          if (e > abs(dy)){
            e -= abs(dy);
            movePenBackward();
            sx++;
          }
        }
      }
    }
  }
}

void loop(){
  getCommand(incoming); //シリアルから読み込み

  char *cmdX, *cmdY, *cmdPen;
  int penUpDown;

  if(incoming){ //座標系、ペン状態別にコマンドを分離
    cmdX = strtok(incoming,",");
    cmdY = strtok(NULL,",");
    cmdPen = strtok(NULL,",");
    penUpDown = atoi(cmdPen);
  }

  if(penUpDown == 0)servoPen.write(PEN_UP_ANGLE); //ペン上げ
  if(penUpDown == 1)servoPen.write(PEN_DOWN_ANGLE); //ペン下げ
  delay(200); //サーボの動作待ち

  x=atoi(cmdX); //charからintへ変換
  y=atoi(cmdY);

  writing(x,y); //指定座標へ移動
  
  Serial.print("X:");
  Serial.print(cmdX); //ペン軸の移動量表示
  Serial.print("/Y:");
  Serial.print(cmdY); //ボール軸の移動量表示
  Serial.print("/Pen:");
  Serial.println(cmdPen); //ペンの上げ下げ状態表示
}

ポイントはブレゼンハムの線分アルゴリズム。
傾きが1以外(1より大きい or 1より小さい)の時も真っ直ぐに線を引くために使いました。

でもって、Processing側はこんな感じの雑なコード。(あくまでも動作確認用なので)
実際は、Arduinoから返ってくる座標とペン状態を受け取ってから次のステップに移るようにする必要がある。

import processing.serial.*;
Serial port;
 
void setup()
{
  size(100,100);
  port = new Serial(this, "/dev/tty.usbmodem1421", 9600);
  port.clear();
}
 
void draw() {
  for(int i=0;i<=4;i++){
  delay(1500);
  port.write("-15,43,1\n");
  delay(1500);
  port.write("38,-30,1\n");
  delay(1500);
  port.write("-45,0,1\n");
  delay(1500);
  port.write("38,30,1\n");
  delay(1500);
  port.write("-15,-43,1\n");
  delay(1500);
  port.write("-1,80,0\n");
  }
  exit();
}

余談
写真の左上にチラッと見えてるパソコンは、最近買ったMacBook Pro Retinaディスプレイモデルのフルカスタムマシン。
こいつのUSBポートから電源を取ると、Arduino UNOにACアダプタを刺さなくても普通に動作しました。