Arduino Nano Every を使ってシャボン玉を作ってみた

ポイント

• サーボの角度、動作スピードを調整するのに苦労した。
• おなじみ「VarSpeedServo」はNano Everyで使用できない。
• そのため自分が得意のPythonで微調整を行った。

Pythonスクリプト

import serial, time

import math

from decimal import Decimal, ROUND_HALF_UP

def translate_angle(x):
    result = 2000*x/180 + 500
    result = Decimal(str(result)).quantize(Decimal('0'), rounding=ROUND_HALF_UP)
    return int(result)

def motion(x):
    servo_1_syo, servo_1_amari = divmod(x[0], 256)
    servo_2_syo, servo_2_amari = divmod(x[1], 256)
    servo_3_syo, servo_3_amari = divmod(x[2], 256)

    delay_time = (x[3]).to_bytes(1, 'big')

    ser.write(servo_1_syo.to_bytes(1, 'big'))
    ser.write(servo_1_amari.to_bytes(1, 'big'))
    ser.write(servo_2_syo.to_bytes(1, 'big'))
    ser.write(servo_2_amari.to_bytes(1, 'big'))
    ser.write(servo_3_syo.to_bytes(1, 'big'))
    ser.write(servo_3_amari.to_bytes(1, 'big'))
    ser.write(delay_time)

def decide_angle(x):
    x1 = 10
    y1 = x  
    r = 10

    a = x1 * 2
    b = y1 *2
    c = -(x1 ** 2 + y1 ** 2)

    x0 = (-a*c - b*(((a*a+b*b)*r*r-c*c)**0.5))/(a*a+b*b)
    y0 = (-c-a*x0)/b

    theta0 = math.degrees(math.atan(y0/x0))
    theta2 = math.degrees(math.atan((y0-y1)/(x1-x0)))
    theta1 = 180 - (theta0 + theta2)

    return translate_angle(180-theta0), translate_angle(theta1), translate_angle(180-theta2)

def slow_motion(x, y, times):
    for i in range(times + 1):
        servo1 = x[0] + ((y[0]-x[0])/times) * i
        servo2 = x[1] + ((y[1]-x[1])/times) * i
        servo3 = x[2] + ((y[2]-x[2])/times) * i

        servo1 = int(Decimal(str(servo1)).quantize(Decimal('0'), rounding=ROUND_HALF_UP))
        servo2 = int(Decimal(str(servo2)).quantize(Decimal('0'), rounding=ROUND_HALF_UP))
        servo3 = int(Decimal(str(servo3)).quantize(Decimal('0'), rounding=ROUND_HALF_UP))

        finished = False
        while not finished:
            finished = (ser.in_waiting != 0)
        ser.reset_input_buffer()
        
        motion([servo1, servo2, servo3, 12])

print("Open Port")
ser =serial.Serial("COM5", 9600)
time.sleep(2.0)

#最初に元の状態を送る
x = [1500, 1500, 1500, 75]
motion(x)
time.sleep(1.0)

return1, return2, return3 = decide_angle(5)

#補正あり
slow_motion(x, [return1, return2, return3-300], 40)

for i in range (50, 90, 2):

    return1, return2, return3 = decide_angle(i/10)

    #補正あり
    x = [return1, return2, return3-300, 12]

    finished = False
    while not finished:
        finished = (ser.in_waiting != 0)
    ser.reset_input_buffer()

    motion(x)

slow_motion(x, [1300, 1500, 1700], 40)

#終了の合図
motion([1300, 1500, 1700, 0])

time.sleep(1.0)

print("Close Port")
ser.close()

Arduinoスケッチ

#include <Servo.h>

Servo myServo_1;
Servo myServo_2;
Servo myServo_3;

int angle_1;
int angle_1_syo;
int angle_1_amari;

int angle_2;
int angle_2_syo;
int angle_2_amari;

int angle_3;
int angle_3_syo;
int angle_3_amari;

int delay_time;

void setup() {
  Serial.begin(9600);

  pinMode(2, OUTPUT);
  myServo_1.attach(3,500,2500);
  myServo_2.attach(9,500,2500);
  myServo_3.attach(10,500,2500);

  myServo_1.writeMicroseconds(1500);
  myServo_2.writeMicroseconds(1500);
  myServo_3.writeMicroseconds(1500);
}

void loop() {
  if(Serial.available()> 6){
    angle_1_syo = Serial.read();
    angle_1_amari = Serial.read();

    angle_2_syo = Serial.read();
    angle_2_amari = Serial.read();
    
    angle_3_syo = Serial.read();
    angle_3_amari = Serial.read();

    delay_time = Serial.read();
    if(delay_time==0){
        digitalWrite(2, HIGH);
        delay(2000);
        digitalWrite(2,LOW);
      }else{
        angle_1 = 256 * angle_1_syo + angle_1_amari;
        angle_2 = 256 * angle_2_syo + angle_2_amari;
        angle_3 = 256 * angle_3_syo + angle_3_amari;

        myServo_1.writeMicroseconds(angle_1);
        myServo_2.writeMicroseconds(angle_2);
        myServo_3.writeMicroseconds(angle_3);

        delay(delay_time*4);

        Serial.write(255);
      }
  }
}