Arduino Yún 加 Motoduino 的網路小車（二）

motoduino.ino

#include <Wire.h>

// Motor pins
int speed_motor1 = 6;  
int speed_motor2 = 5;
int direction_motor1 = 7;
int direction_motor2 = 8;

// Sensor pins
int distance_sensor = A0;

void setup() {
  for(int i=5;i<=8;i++) {
      pinMode(i, OUTPUT);  
  }
  
  // Join i2c bus with address #4 & receive events
  Wire.begin(4);                
  Wire.onReceive(receiveEvent);     
}

void loop() {
    delay(100);
}

// Function that executes whenever data is received from the Yun
void receiveEvent(int howMany) {
    // Read all data for motor commands
    int pwm1 = Wire.read(); Wire.read(); //,
    int dir1 = Wire.read(); Wire.read();
  
    int pwm2 = Wire.read(); Wire.read();
    int dir2 = Wire.read();

    send_motor_command(speed_motor1, direction_motor1, pwm1, dir1);
    send_motor_command(speed_motor2, direction_motor2, pwm2, dir2);
}

// Function to command a given motor of the robot
void send_motor_command(int speed_pin, int direction_pin, int pwm, boolean dir) {
    analogWrite(speed_pin,pwm); // Set PWM control, 0 for stop, and 255 for maximum speed
    digitalWrite(direction_pin,dir);
}