mzservom.nc

tinyos最新版

/*									tab:4
 *
 *
 * "Copyright (c) 2002 and The Regents of the University 
 * of California.  All rights reserved.
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose, without fee, and without written agreement is
 * hereby granted, provided that the above copyright notice, the following
 * two paragraphs and the author appear in all copies of this software.
 * 
 * IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY FOR
 * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
 * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF
 * CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * 
 * THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
 * AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
 *
 * Authors:		Sarah Bergbreiter
 * Date last modified:  8/12/02
 *
 * Uses the ADC to sense current position and Motor2 to drive the motor
 * to the desired position.
 *
 * NOTE: This module uses the Timer1 Overflow Interrupt.
 *
 */

module MZServoM {
  provides {
    interface Servo;
    interface ServoCalibration;
  }
  uses {
    interface HPLMotor as Motor;
    interface ADC;
    interface ADCControl;
    interface EEPROM;
    interface Interrupt;
  }
}
implementation {

  uint8_t straight;
  uint8_t Kp;
  uint8_t Ki;
  uint8_t servoReference;
  uint16_t totalError;
  uint16_t count;
  uint8_t debug;
  uint8_t servoControlCnt;
  uint16_t servoData;

  enum {
    KP_ADDR = 10,
    KI_ADDR = 11,
    STRAIGHT_ADDR = 12
  };

  enum {
    MAX_SPEED = 255,
    OFF = 0,
    REVERSE = 0,
    FORWARD = 1
  };

  /* Set straight variable */
  command result_t ServoCalibration.setStraight(uint8_t newStraight) {
    straight = newStraight;
    call Interrupt.disable();
    call EEPROM.write(STRAIGHT_ADDR,straight);
    call Interrupt.enable();
    return SUCCESS;
  }

  /* Set Kp */
  command result_t ServoCalibration.setKp(uint8_t newKp) {
    Kp = newKp;
    call Interrupt.disable();
    call EEPROM.write(KP_ADDR,Kp);
    call Interrupt.enable();
    return SUCCESS;
  }

  /* Set Ki */
  command result_t ServoCalibration.setKi(uint8_t newKi) {
    Ki = newKi;
    call Interrupt.disable();
    call EEPROM.write(KI_ADDR,Ki);
    call Interrupt.enable();
    return SUCCESS;
  }

  /* Get straight variable */
  command uint8_t ServoCalibration.getStraight() {
    return call EEPROM.read(STRAIGHT_ADDR);
  }

  /* Get Kp */
  command uint8_t ServoCalibration.getKp() {
    return call EEPROM.read(KP_ADDR);
  }

  /* Get Ki */
  command uint8_t ServoCalibration.getKi() {
    return call EEPROM.read(KI_ADDR);
  }

  /* Set Debug */
  command result_t ServoCalibration.setDebug(uint8_t state) {
    debug = state;
    return SUCCESS;
  }

  /* Initialize Servo components and enable Timer1 interrupt */
  command result_t Servo.init() {
    call EEPROM.init();

    /* Read constants from EEPROM */
    call Interrupt.disable();
    Kp = call EEPROM.read(KP_ADDR);
    Ki = call EEPROM.read(KI_ADDR);
    straight = call EEPROM.read(STRAIGHT_ADDR);
    call Interrupt.enable();

    /* Set default constants */
    sbi(TIMSK,TOIE1);
    if (straight == 255) straight = 170;
    if (Kp == 255) Kp = 96;
    if (Ki == 255) Ki = 4;
//    straight = 170;
//    Kp = 96;
//    Ki = 4;

    servoReference = straight;
    totalError = 0;
    debug = 0;
    servoControlCnt = 0;
    call Motor.init();
    call ADCControl.init();
    return SUCCESS;
  }

  /* Set the current turn on the servo */
  command result_t Servo.setTurn(uint8_t turn) {
    if (turn > 60) servoReference = straight + 30;
    else servoReference = straight - 30 + turn;
    totalError = 0;
    return SUCCESS;
  }

  task void servoControl() {
    uint16_t iErr = 0;
    uint8_t error;
    uint8_t control;

    /* Find error between actual and desired position */
    servoData = servoData >> 1;
    if (servoData > servoReference) {
      error = servoData - servoReference;
      call Motor.setDir(REVERSE);
    } else {
      error = servoReference - servoData;
      call Motor.setDir(FORWARD);
    }

    /* Add to integral error */
    totalError += error;

    /* Choose desired speed based on PI portions of control loop */
    if (error > 0) {
      if (Kp < 136) error = (error*Kp) >> 5;
      else error = 255;
      iErr = (totalError*Ki) >> 5;
      if (error+iErr < MAX_SPEED) {
        call Motor.setSpeed(error + iErr);
        control = error+iErr;
      } else {
        control = MAX_SPEED;
        call Motor.setSpeed(MAX_SPEED);
      }
    } else {
      totalError = 0;
      call Motor.setSpeed(OFF);
      control = 0;
    }

    if (debug) {
      count++;
      if (count == 3) {
        signal Servo.debug(control);
        count = 0;
      }
    }
  }

  /* PID loop to control Motor2.  Signals an event with sensed position */
  event result_t ADC.dataReady(uint16_t data) {
    servoData = data;
    post servoControl();
    return SUCCESS;
  }

  /* Sample the ADC at 1/3 rate of motor PWM */
  TOSH_INTERRUPT(SIG_OVERFLOW1) {
    servoControlCnt++;
    if (servoControlCnt == 3) {
      call ADC.getData();
      servoControlCnt = 0;
    }
  }

  /* EEPROM Write is finished */
  event result_t EEPROM.writeDone() {
    return SUCCESS;
  }

}