週刊中ロボ137 「ナノ四駆」のスケッチ

「ナノ四駆」のスケッチ です。

Arduiroid3Test.inoとして保存して下さい。
 

// Arduiroid [Nano4Runner] Robot　0.0 nakarobo 2021.1.30
// File Arduroid3Test.ino

// Servo
#include <Servo.h>

#define MAX_SERVO_NUM 4
Servo servo[MAX_SERVO_NUM+1];

bool servoOn; // true:Servo Power On false:Off

//Arduroid3 (Top View)
//FRONT
//  -----   ----- 
// |  1  | |  2  |
// | P04 | | P05 |
//  -----   -----
//
//  -----   -----
// |  3  | |  4  |
// | P06 | | P07 |
//  -----   ----- 
//BACK

// Setup First Power On at Once
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  while (!Serial);

  pinMode(LED_BUILTIN, OUTPUT);
 
  // Servo Test
  servoOn = false;
  attachServo();
  detachServo(); 

}

// MainLoop
void loop() {
  // if there's any serial available, read it:
  while (Serial.available() > 0)
  {
    char command = Serial.read();
    switch (command){
      case '0': // DEMO
        set4Pos( 80, 80,100,100); delay(500);
        set4Pos( 100,100,80,80); delay(500);
        set4Pos( 60, 60,120,120); delay(500);
        set4Pos( 120,120,60,60); delay(500);
        set4Pos( 45, 45,135,135); delay(500);
        set4Pos( 135,135,45,45); delay(500);
        set4Pos( 90, 90, 90, 90); delay(100);
        break;
      case '1': // Right
        set4Pos(120,110,120,120); delay(100);
        set4Pos(125,110,125,130); delay(200);
        set4Pos( 90, 90,125,130); delay(200);
        set4Pos( 90, 90, 90, 90); delay(100);
        break;
      case '2': // Foward
        set4Pos( 90, 90, 90, 90); delay(100);
        set4Pos( 45, 80,120, 70); delay(200);
        set4Pos( 80, 90,120,120); delay(200);
        set4Pos( 90, 90, 90, 90); delay(200);
        set4Pos(100,135,110, 60); delay(200);
        set4Pos(100,100, 60, 60); delay(200);
        set4Pos( 90, 90, 90, 90); delay(100);
        break;
      case '3': // Left
        set4Pos( 80, 60, 60, 60); delay(100);
        set4Pos( 80, 55, 45, 55); delay(200);
        set4Pos( 90, 90, 45, 55); delay(200);
        set4Pos( 90, 90, 90, 90); delay(100);
        break;
      case '4': // Lay down
        set4Pos( 150, 30,120, 60); delay(100);
        set4Pos(170, 10,150, 30); delay(300);
        break;
      case '5': // Stand up
        set4Pos( 70,110,110, 70); delay(100);
        set4Pos( 90, 90, 90, 90); delay(300);
        break;
      case '6': // Ready
        set4Pos(100, 10,150, 45); delay(200);
        set4Pos( 45, 20,150, 30); delay(100);
        break;
      case '7': // Hi-Hi
        set4Pos( 50,70,160, 20); delay(100);
        set4Pos(110, 70,160, 30); delay(300);
        set4Pos(110,130,150, 20); delay(300);
        set4Pos( 50,130,160, 20); delay(100);
        break;
      case '8': // Back
        set4Pos( 90, 90, 90, 90); delay(100);
        set4Pos( 90, 90, 60, 30); delay(200);
        set4Pos( 90,135, 60, 90); delay(200);
        set4Pos( 90, 90, 90, 90); delay(200);
        set4Pos( 90, 90,150,120); delay(200);
        set4Pos( 45, 90, 90,120); delay(200);
        set4Pos( 90, 90, 90, 90); delay(100);
        break;
      case '9': // Junp
        set4Pos( 50,130, 45,135); delay(450);
        set4Pos( 90, 90, 90, 90); delay(200);
        set4Pos(100, 80,170, 10); delay(100);
        set4Pos( 90, 90, 90, 90); delay(100);
        break;
      case 'T': // Test Run
      case 't':
        for(int i=0; i<5; i++)
        {
          set4Pos( 90, 90, 90, 90); delay(25);
          set4Pos( 45, 80,120, 70); delay(75);
          set4Pos( 80, 90,120,120); delay(75);
          set4Pos( 90, 90, 90, 90); delay(75);
          set4Pos(100,135,110, 60); delay(75);
          set4Pos(100,100, 60, 60); delay(75);
          set4Pos( 90, 90, 90, 90); delay(25);
        }
        break;
        
      case 'O': // Serbo On/OFF
      case 'o':        
        if( servoOn )
        {
          detachServo();
          servoOn = false;
        } else {
          attachServo();
          servoOn = true;
        }
        break;
      case 'D': // Wait
      case 'd':
        delay(300);
        break;
     } // switch

    // SERVO Set command
    if ( command == 'S' || command == 's' )  // 'S' is Servo Command "S,Servo_no,Angle"
    {
      Serial.print(command);
      Serial.print(',');
      int servoNo = Serial.parseInt();
      Serial.print(servoNo);
      Serial.print(',');
      int servoAngle = Serial.parseInt();
      Serial.print(servoAngle);
      Serial.println();

      if ( 0 <= servoNo &&  servoNo < MAX_SERVO_NUM+1 )
      {
        // Single Servo Move
        servo[servoNo].write(servoAngle);

        // Wait Servo Move
        delay(300); // 180/60(°/100msec）=300(msec)
      }
    }  // SERVO Set END

    // SERVO All command
    if ( command == 'A' || command == 'a' )  // 'A' is Servo All Command "A,Angle0, Angle1...Angle3"
    {
      Serial.print(command);

      // read Angle from Serial
      int angle[MAX_SERVO_NUM];
      for (int i = 0; i < MAX_SERVO_NUM; i++)
      {
        angle[i] = Serial.parseInt();
        Serial.print(',');
        Serial.print(angle[i]);
      }
      Serial.println();

      // servo Move
      set4Pos( angle[0], angle[1], angle[2], angle[3]);
      
      // Wait Servo Move
      delay(300);

    }   // SERVO All End
    
  } // End while

} // End loop

// attach ServoMotors
void attachServo()
{
  // attach Pin to ServoMotor
// Tower Pro SG90 500-1450-2400
  servo[1].attach(4, 500, 2400);
  servo[2].attach(5, 500, 2400);
  servo[3].attach(6, 500, 2400);
  servo[4].attach(7, 500, 2400);
// Miuzei MS18 600-1500-2400
//  servo[1].attach(4, 600, 2400);
//  servo[2].attach(5, 600, 2400);
//  servo[3].attach(6, 600, 2400);
//  servo[4].attach(7, 600, 2400);
    
  servoOn = true;

  // LED ON
  digitalWrite(LED_BUILTIN, HIGH);
}

// dettach ServoMotors
void detachServo()
{
  servo[1].detach();
  servo[2].detach();
  servo[3].detach();
  servo[4].detach();
  servoOn = false;

  // LED OFF
  digitalWrite(LED_BUILTIN, LOW); 
}

// Set 4 ServoMotor angles
void set4Pos(int a, int b, int c, int d){
  servo[1].write(a);
  servo[2].write(b);
  servo[3].write(c);
  servo[4].write(d);
}
// End
//---------------------

公開した映像はこのスケッチを使ったものです。