Motor driver for class 3400's exterior of Kuniko-sai on 2012/09/08(Sat) - 2012/09/09(Sun)

sketch_motordriver2.cpp

/*
 * Description:    Motor driver for class 3400's exterior of
                   Kuniko-sai on 2012/09/08(Sat) - 2012/09/09(Sun)
 * Last modified:  2012/09/03
 */

//#define DEBUG
//#define DEBUG_SERIAL

/******************************************************************/
// macro definitions
/******************************************************************/
#define  M_is_pin_LOW(pin)  (digitalRead(pin) == LOW)
#define  M_is_pin_HIGH(pin)  (digitalRead(pin) == HIGH)

#define  SPEED_KEEP  (-1)
#define  MOTOR1_SPEED_MAX  (255)

/******************************************************************/
// constant values
/******************************************************************/
/*** PIN ***/
//! pin numbers
enum pin_number {
  PIN_VR1 = 1,                //!< [in] variable register 1 (analog, 0 - 1023)
  PIN_MOTOR1_R = 3,           //!< [out] motor 1 right
  PIN_MOTOR1_L = 4,           //!< [out] motor 1 left
  PIN_MOTOR1 = 5,             //!< [out] motor 1 rotating speed (analog, 0 - 255)
  PIN_LIGHT1 = 6,             //!< [out] light 1 (HIGH to on)
  PIN_ROT1_1 = 7,             //!< [in] rotation switch 1 (LOW when on)
  PIN_ROT1_2 = 8,             //!< [in] rotation switch 1 (LOW when on)
  PIN_SWITCH1_LEFT = 9,       //!< [in] switch 1 (LOW when on)
  PIN_SWITCH1_RIGHT = 10,     //!< [in] switch 1 (LOW when on)
  PIN_SENSOR1 = 11,           //!< [in] sensor 1 (switch, HIGH when on)
  PIN_SENSOR2 = 12,           //!< [in] sensor 2 (switch, HIGH when on)
}; // enum pin_number

//! pin aliases
enum {
  // pin in auto mode
  PIN_AUTO_CLOSE_TIME_SETTING = PIN_SWITCH1_LEFT,
  PIN_AUTO_OPEN_TIME_SETTING = PIN_SWITCH1_RIGHT,
  PIN_AUTO_FAST_MODE = PIN_SWITCH1_LEFT,
  PIN_AUTO_SPEED_SETTING = PIN_SWITCH1_RIGHT,
  // pin in manual mode
  PIN_MANUAL_MOVE_R = PIN_SWITCH1_RIGHT,
  PIN_MANUAL_MOVE_L = PIN_SWITCH1_LEFT,
  PIN_MOVESPEED = PIN_VR1,
  PIN_WAITINGTIME = PIN_VR1,
  PIN_SENSOR_L = PIN_SENSOR1,
  PIN_SENSOR_R = PIN_SENSOR2
};
/*** others ***/
//! modes
enum {
  PIN_AUTOMODE1 = PIN_ROT1_1,
  PIN_AUTOMODE2 = PIN_ROT1_2,
  MODE_DUMMY = 129,
  PIN_MANUALMODE
} mode, previous_mode;

//! motor states
enum {
  MOVING_RIGHT,
  MOVING_LEFT,
  KEEP_STOPPING_AT_RIGHT,
  KEEP_STOPPING_AT_LEFT,
  AUTOMODE_INIT
} motor_direction = AUTOMODE_INIT;

// sensor
enum sensor_lr {
  SENSOR_LEFT,
  SENSOR_RIGHT,
};

// direction
enum direction {
  LEFT,
  RIGHT
};


/******************************************************************/
// global variables
/******************************************************************/
// speed of motor 1
//   default :  (0 - 255)
unsigned int speed_custom = 128;
// time to keep stopping at right end (in auto mode)
//   default 20000 ms
#ifndef DEBUG
unsigned long waitingtime_open = 20000; /* ms */
#else
unsigned long waitingtime_open = 2000; /* ms */
#endif
// time to keep stopping at left end (in auto mode)
//   default 10000 ms
#ifndef DEBUG
unsigned long waitingtime_close = 10000; /* ms */
#else
unsigned long waitingtime_close = 1000; /* ms */
#endif
// used in auto mode
enum sensor_lr last_responded_sensor;
enum sensor_lr sensor_expected;

/******************************************************************/
// prototype declerations
/******************************************************************/
//! initialize hardware and software. [system]
void setup();
//! loop [system]
void loop();
//! auto mode 1
void mode_auto1();
//! auto mode 2
void mode_auto2();
//! manual mode
void mode_manual();
//! drive motor and light in auto mode
void mode_auto_run(signed int speed, unsigned long left_waiting, unsigned long right_waiting);
/*** utilities ***/
//! drive motor 1 (in auto mode)
void drive_motor1(enum direction dir, signed int speed, void (*drive_light)(void *), void *light_arg_ptr);
//! operate motor1
void motor1_move_left(uint8_t speed);
//! operate motor1
void motor1_move_right(uint8_t speed);
//! stop motor1
void motor1_stop();
//! blink light
void blink_light1(void *args);


/******************************************************************/
// functions
/******************************************************************/

//! initialize hardware and software. [system]
void setup() {
  const enum pin_number output_pins[] = {
    PIN_MOTOR1_L, PIN_MOTOR1_R,
    PIN_MOTOR1, PIN_LIGHT1
  };
  const enum pin_number input_pins_pullup[] = {
    PIN_ROT1_1, PIN_ROT1_2,
    PIN_SWITCH1_LEFT, PIN_SWITCH1_RIGHT,
    PIN_SENSOR1, PIN_SENSOR2
  };
  // pin setup
  for(int i=0; i< (sizeof(output_pins) / sizeof(enum pin_number)); ++i) {
    pinMode(output_pins[i], OUTPUT);
  }
  for(int i=0; i< (sizeof(input_pins_pullup) / sizeof(enum pin_number)); ++i) {
    pinMode(input_pins_pullup[i], INPUT_PULLUP);
  }
  // mode setup
  previous_mode = MODE_DUMMY;
  mode = MODE_DUMMY;
  // reset light
  digitalWrite(PIN_LIGHT1, HIGH);
#ifdef DEBUG_SERIAL
    Serial.begin(9600);
digitalWrite(PIN_LIGHT1, HIGH);
while(delay(100),1){Serial.println(M_is_pin_LOW(PIN_SWITCH1_LEFT));}
#endif
#ifdef DEBUG
boolean a = true;
while(delay(500),1){digitalWrite(PIN_LIGHT1, (a?HIGH:LOW));a=!a;}
#endif
} //void setup()


//! loop [system]
void loop() {
  if(M_is_pin_LOW(PIN_AUTOMODE1)) {
    mode = PIN_AUTOMODE1;
    mode_auto1();
  } else if(M_is_pin_LOW(PIN_AUTOMODE2)) {
    mode = PIN_AUTOMODE2;
    mode_auto2();
  } else {
    // otherwise, manual mode
    mode = PIN_MANUALMODE;
    mode_manual();
  }
/*
  switch(mode) {
    case  AUTOMODE1:
      mode_auto1();
      break;
    case  AUTOMODE2:
      mode_auto2();
      break;
//    case  MANUALMODE:
    default:
      mode_manual();
  }
*/
} // void loop()


//! auto mode 1
void mode_auto1() {
  // internal modes
  enum {
    CUSTOM_MODE,
    CLOSE_TIME_SETTING_MODE,
    OPEN_TIME_SETTING_MODE
  } internal_mode = CUSTOM_MODE;
  if((mode != previous_mode) && (previous_mode == PIN_AUTOMODE2)) {
    // mode chanded. initialize states here.
    motor_direction = AUTOMODE_INIT;
    last_responded_sensor = SENSOR_LEFT;
    sensor_expected = SENSOR_RIGHT;
    // reset light
    digitalWrite(PIN_LIGHT1, LOW);
    // initialization completed.
    previous_mode = mode;
  }
  if(M_is_pin_LOW(PIN_AUTO_CLOSE_TIME_SETTING)) {
    // waiting time setting mode
    //   analogRead() : 0 - 1023
    //   waitingtime : 0 - 61380 /*ms*/
    //     61380 == 1023 * 60
    internal_mode = CLOSE_TIME_SETTING_MODE;
    // Note that analogRead() returns signed int value.
    waitingtime_close = ((unsigned long)analogRead(PIN_WAITINGTIME)) * 60;
  } else if(M_is_pin_LOW(PIN_AUTO_OPEN_TIME_SETTING)) {
    // waiting time setting mode
    //   analogRead() : 0 - 1023
    //   waitingtime : 0 - 61380 /*ms*/
    //     61380 == 1023 * 60
    internal_mode = OPEN_TIME_SETTING_MODE;
    // Note that analogRead() returns signed int value.
    waitingtime_open = ((unsigned long)analogRead(PIN_WAITINGTIME)) * 60;
  }
  switch(internal_mode) {
    case  CUSTOM_MODE:
      mode_auto_run(speed_custom, waitingtime_close, waitingtime_open);
      break;
    case  CLOSE_TIME_SETTING_MODE:
    case  OPEN_TIME_SETTING_MODE:
    {
      boolean is_closing = (internal_mode == CLOSE_TIME_SETTING_MODE);
      unsigned long waitingtime = (is_closing ? waitingtime_close : waitingtime_open) / 5;
      drive_motor1((is_closing ? LEFT : RIGHT), SPEED_KEEP, blink_light1, &waitingtime);
      break;
    }
  } // switch
} // void mode_auto1()


//! auto mode 2
void mode_auto2() {
  // internal modes
  enum {
    CUSTOM_MODE,
    SPEED_SETTING_MODE,
    FAST_MODE
  } internal_mode = CUSTOM_MODE;
  if((mode != previous_mode) && (previous_mode != PIN_AUTOMODE1)) {
    // mode chanded. initialize states here.
    motor_direction = AUTOMODE_INIT;
    last_responded_sensor = SENSOR_LEFT;
    sensor_expected = SENSOR_RIGHT;
    // reset light
    digitalWrite(PIN_LIGHT1, LOW);
    // initialization completed.
    previous_mode = mode;
  }
  if(M_is_pin_LOW(PIN_AUTO_FAST_MODE)) {
    internal_mode = FAST_MODE;
  } else if(M_is_pin_LOW(PIN_AUTO_SPEED_SETTING)) {
    // motor speed setting mode
    // analogRead() : 0 - 1023
    // speed : 0 - 255
    internal_mode = SPEED_SETTING_MODE;
    speed_custom = analogRead(PIN_MOVESPEED) >> 2;
  }
  switch(internal_mode) {
    case  CUSTOM_MODE:
      mode_auto_run(speed_custom, waitingtime_close, waitingtime_open);
      break;
    case  FAST_MODE:
      mode_auto_run(MOTOR1_SPEED_MAX, waitingtime_close, waitingtime_open);
      break;
    case  SPEED_SETTING_MODE:
      mode_auto_run(speed_custom, 0, 0);
      break;
  } // switch
} // void mode_auto2()


//! manual mode
void mode_manual() {
  unsigned long speed_manual = analogRead(PIN_MOVESPEED) >> 2;
  int light = LOW;
  if(mode != previous_mode) {
    // mode chanded. initialize states here.
    digitalWrite(PIN_LIGHT1,LOW);
    motor1_stop();
    // initialization completed.
    previous_mode = mode;
  }
  if(M_is_pin_LOW(PIN_MANUAL_MOVE_L) && M_is_pin_LOW(PIN_SENSOR_L)) {
    motor1_move_left(speed_manual);
  } else if(M_is_pin_LOW(PIN_MANUAL_MOVE_R) && M_is_pin_LOW(PIN_SENSOR_R)) {
    motor1_move_right(speed_manual);
  } else {
    light = HIGH;
    motor1_stop();
  }
  digitalWrite(PIN_LIGHT1, light);
} // void mode_manual()


//! drive motor and light in auto mode
/*!
 * \param speed         Speed of rotation of motor 1 (0 - 255).
 *                      Pass negative value to keep current speed.
 * \param left_waiting  Waiting time at left end millisec.
 * \param left_waiting  Waiting time at right end in millisec.
 */
void mode_auto_run(signed int speed, unsigned long left_waiting, unsigned long right_waiting) {
  static unsigned long previousMillis = 0;
  unsigned long currentMillis;
  static signed int prev_speed = 0;
  if(motor_direction == AUTOMODE_INIT) {
    // change mode
    motor1_move_right(speed);
    motor_direction = MOVING_RIGHT;
  }
  currentMillis = millis();
  if(motor_direction == KEEP_STOPPING_AT_LEFT) {
    if(currentMillis - previousMillis >= left_waiting) {
      // it is time to move
      // move right
      motor1_move_right(speed);
      motor_direction = MOVING_RIGHT;
    }
  } else if(motor_direction == KEEP_STOPPING_AT_RIGHT) {
    if(currentMillis - previousMillis >= right_waiting) {
      // it is time to move
      // move left
      motor1_move_left(speed);
      motor_direction = MOVING_LEFT;
      // and turn the light off
      digitalWrite(PIN_LIGHT1, LOW);
    }
    return;
  }
  if(M_is_pin_HIGH((sensor_expected == SENSOR_LEFT) ? PIN_SENSOR_L : PIN_SENSOR_R)) {
    boolean is_left_responded = (sensor_expected == SENSOR_LEFT);
    // stopped right now
    // state : stop
    motor_direction = (is_left_responded ? KEEP_STOPPING_AT_LEFT : KEEP_STOPPING_AT_RIGHT);
    // reset timer
    previousMillis = currentMillis;
    // motor off
    motor1_stop();
    // set last pushed switch
    last_responded_sensor = sensor_expected;
    sensor_expected = (is_left_responded ? SENSOR_RIGHT : SENSOR_LEFT);
    if(!is_left_responded) {
      digitalWrite(PIN_LIGHT1, HIGH);
    }
    return;
  }
  if(speed < 0) {
    speed = prev_speed;
  }
  if(prev_speed != speed) {
    analogWrite(PIN_MOTOR1, ((speed >= 0) ? speed : prev_speed));
    prev_speed = speed;
  }
} // void mode_auto_time_setting()


//! drive motor 1 (in auto mode)
void drive_motor1(enum direction dir, signed int speed, void (*drive_light)(void *), void *light_args_ptr) {
  int sensor_pin_expected = ((dir == LEFT) ? PIN_SENSOR_L : PIN_SENSOR_R);
  if(M_is_pin_HIGH(sensor_pin_expected)) {
    // reached to the end
    motor1_stop();
  } else {
    // moving... (silently)
    ((dir == LEFT) ? motor1_move_left : motor1_move_right)(speed);
  }
  // call light driver
  drive_light(light_args_ptr);
} // drive_motor1(enum direction, signed int, void (*)(void *), void *)


//! operate motor1
/*!
 * \param speed Speed of rotation (0 - 255)
 */
void motor1_move_left(uint8_t speed) {
  digitalWrite(PIN_MOTOR1_R, LOW);
  digitalWrite(PIN_MOTOR1_L, HIGH);
  analogWrite(PIN_MOTOR1, speed);
}

//! operate motor1
/*!
 * \param speed Speed of rotation (0 - 255)
 */
void motor1_move_right(uint8_t speed) {
  digitalWrite(PIN_MOTOR1_L, LOW);
  digitalWrite(PIN_MOTOR1_R, HIGH);
  analogWrite(PIN_MOTOR1, speed);
}

//! stop motor1
void motor1_stop() {
  digitalWrite(PIN_MOTOR1_L, LOW);
  digitalWrite(PIN_MOTOR1_R, LOW);
  analogWrite(PIN_MOTOR1, 0);
}

//! blink light
void blink_light1(void *args) {
  static unsigned long previousMillis = millis();
  static boolean light_on = true;
  unsigned long currentMillis = millis();
  unsigned long interval = *(unsigned long *)args;
  if(currentMillis - previousMillis >= interval) {
    digitalWrite(PIN_LIGHT1, (light_on ? LOW : HIGH));
    light_on = !light_on;
    previousMillis = currentMillis;
  }
}