Robofun 機器人論壇

標題: 請問哪裡是可以調整 servo轉動的地方 [打印本頁]

作者: tracy    時間: 2011-10-12 16:15
標題: 請問哪裡是可以調整 servo轉動的地方
// Includes
#include <avr/interrupt.h>
#include <avr/io.h>
#include <stdlib.h>
#include "oi.h"

//AVRlib includes
#include "global.h"

#include "a2d.h"



// Constants
#define RESET_SONG 0
#define START_SONG 1
#define BUMP_SONG  2
#define END_SONG   3




#define PORT_ON( port_letter, number )
port_letter |= (1<<number)
#define PORT_OFF( port_letter, number )
port_letter &= ~(1<<number)
#define PORT_ALL_ON( port_letter, number )
port_letter |= (number)
#define PORT_ALL_OFF( port_letter, number )
port_letter &= ~(number)
#define FLIP_PORT( port_letter, number )
port_letter ^= (1<<number)
#define PORT_IS_ON( port_letter, number )
( port_letter & (1<<number) )
#define PORT_IS_OFF( port_letter, number )
!( port_letter & (1<<number) )



// Global variables
volatile uint16_t timer_cnt = 0;
volatile uint8_t timer_on = 0;
volatile uint8_t sensors_flag = 0;
volatile uint8_t sensors_index = 0;
volatile uint8_t sensors_in[Sen6Size];
volatile uint8_t sensors[Sen6Size];
volatile int16_t distance = 0;
volatile int16_t angle = 0;
volatile uint16_t update_delay = 15;
volatile uint8_t range = 0;

unsigned int scan_thresh=0;
unsigned long int scan_angle=750;


void byteTx(uint8_t value);
void delayMs(uint16_t time_ms);
void delayAndUpdateSensors(unsigned int time_ms);
void initialize(void);
void powerOnRobot(void);
void baud(uint8_t baud_code);
void drive(int16_t velocity, int16_t radius);
void drive_distance(int16_t velocity, int16_t radius, int16_t distance_togo);
void drive_angle(int16_t velocity, int16_t radius, int16_t angle_togo);
uint16_t randomAngle(void);
void defineSongs(void);


void rotate_CCW(int16_t angle_tmp, int16_t velocity);
void rotate_CW(int16_t angle_tmp, int16_t velocity);
void straight(int16_t distance_tmp, int16_t velocity);
void stop(void);
void scan(unsigned int scan_thresh);
void servo_scan(unsigned long int speed);
void delay_cycles(unsigned long int cycles);


int main (void)
{
  uint8_t leds_cnt = 99;
  uint8_t leds_state = 0;
  uint8_t leds_on = 1;



  initialize();
  LEDBothOff;
  powerOnRobot();
  byteTx(CmdStart);
  baud(Baud28800);
  defineSongs();
  byteTx(CmdControl);
  byteTx(CmdFull);


  drive(0, RadStraight);


  byteTx(CmdPlay);
  byteTx(RESET_SONG);
  delayAndUpdateSensors(750);


  for(;;)
  {

    if(++leds_cnt >= 100)
    {
      leds_cnt = 0;
      leds_on = !leds_on;

      if(leds_on)
      {
        byteTx(CmdLeds);
        byteTx(LEDsBoth);
        byteTx(128);
        byteTx(255);
        LEDBothOff;
      }
      else
      {
        byteTx(CmdLeds);
        byteTx(0x00);
        byteTx(0);
        byteTx(0);
        LEDBothOn;
      }
    }

    delayAndUpdateSensors(10);

    if(UserButtonPressed)
    {
      byteTx(CmdPlay);
      byteTx(START_SONG);
      delayAndUpdateSensors(2500);


      angle = 0;

  distance = 0;

      while(!(UserButtonPressed)
            && (!sensors[SenCliffL])
            && (!sensors[SenCliffFL])
            && (!sensors[SenCliffFR])
            && (!sensors[SenCliffR])
            && (!sensors[SenChAvailable]))
      {





scan(30);


if (scan_angle > 840)

drive(100, RadCW);

else if (scan_angle < 660)t

drive(100, RadCCW);

else

{

if (a2dConvert8bit(1) > 50)

drive(-300, RadStraight);

else

drive(100, RadStraight);

}








        delayAndUpdateSensors(update_delay);
      }

      // Stop driving
      drive(0, RadStraight);

      // Play end song and wait
      delayAndUpdateSensors(500);
      byteTx(CmdPlay);
      byteTx(END_SONG);
      delayAndUpdateSensors(2000);

    }
  }
}




// Serial receive interrupt to store sensor values
SIGNAL(SIG_USART_RECV)
{
  uint8_t temp;


  temp = UDR0;

  if(sensors_flag)
  {
    sensors_in[sensors_index++] = temp;
    if(sensors_index >= Sen6Size)
      sensors_flag = 0;
  }
}




SIGNAL(SIG_OUTPUT_COMPARE1A)
{
  if(timer_cnt)
    timer_cnt--;
  else
    timer_on = 0;
}





void byteTx(uint8_t value)
{
  while(!(UCSR0A & _BV(UDRE0))) ;
  UDR0 = value;
}





void delayMs(uint16_t time_ms)
{
  timer_on = 1;
  timer_cnt = time_ms;
  while(timer_on) ;
}




void delayAndUpdateSensors(uint16_t time_ms)
{
  uint8_t temp;

  timer_on = 1;
  timer_cnt = time_ms;
  while(timer_on)
  {
    if(!sensors_flag)
    {
      for(temp = 0; temp < Sen6Size; temp++)
        sensors[temp] = sensors_in[temp];


      distance += (int)((sensors[SenDist1] << 8) | sensors[SenDist0]);
      angle += (int)((sensors[SenAng1] << 8) | sensors[SenAng0]);

      byteTx(CmdSensors);
      byteTx(6);
      sensors_index = 0;
      sensors_flag = 1;
    }
  }
  range=a2dConvert8bit(1);
}





void initialize(void)
{
  cli();

  // Set I/O pins
  DDRB = 0x12;

  PORTB = 0xCF;
  DDRC = 0x00;  
  PORTC = 0x00;
  DDRD = 0xE6;  
  PORTD = 0x7D;


  TCCR1A = 0x00;
  TCCR1B = (_BV(WGM12) | _BV(CS12));
  OCR1A = 71;
  TIMSK1 = _BV(OCIE1A);


  UBRR0 = 19;
  UCSR0B = (_BV(RXCIE0) | _BV(TXEN0) | _BV(RXEN0));
  UCSR0C = (_BV(UCSZ00) | _BV(UCSZ01));



  a2dInit();
  a2dSetPrescaler(ADC_PRESCALE_DIV32);
  a2dSetReference(ADC_REFERENCE_AVCC);


  sei();
}




void powerOnRobot(void)
{

  if(!RobotIsOn)
  {
      while(!RobotIsOn)
      {
          RobotPwrToggleLow;
          delayMs(500);  
          RobotPwrToggleHigh;
          delayMs(100);
          RobotPwrToggleLow;
      }
      delayMs(3000);
  }
}





void baud(uint8_t baud_code)
{
  if(baud_code <= 11)
  {
    byteTx(CmdBaud);
    UCSR0A |= _BV(TXC0);
    byteTx(baud_code);

    while(!(UCSR0A & _BV(TXC0))) ;

    cli();


    if(baud_code == Baud115200)
      UBRR0 = Ubrr115200;
    else if(baud_code == Baud57600)
      UBRR0 = Ubrr57600;
    else if(baud_code == Baud38400)
      UBRR0 = Ubrr38400;
    else if(baud_code == Baud28800)
      UBRR0 = Ubrr28800;
    else if(baud_code == Baud19200)
      UBRR0 = Ubrr19200;
    else if(baud_code == Baud14400)
      UBRR0 = Ubrr14400;
    else if(baud_code == Baud9600)
      UBRR0 = Ubrr9600;
    else if(baud_code == Baud4800)
      UBRR0 = Ubrr4800;
    else if(baud_code == Baud2400)
      UBRR0 = Ubrr2400;
    else if(baud_code == Baud1200)
      UBRR0 = Ubrr1200;
    else if(baud_code == Baud600)
      UBRR0 = Ubrr600;
    else if(baud_code == Baud300)
      UBRR0 = Ubrr300;

    sei();

    delayMs(100);
  }
}





void drive(int16_t velocity, int16_t radius)
{
  byteTx(CmdDrive);
  byteTx((uint8_t)((velocity >> 8) & 0x00FF));
  byteTx((uint8_t)(velocity & 0x00FF));
  byteTx((uint8_t)((radius >> 8) & 0x00FF));
  byteTx((uint8_t)(radius & 0x00FF));
}





// Define songs to be played later
void defineSongs(void)
{
  // Reset song
  byteTx(CmdSong);
  byteTx(RESET_SONG);
  byteTx(4);
  byteTx(60);
  byteTx(6);
  byteTx(72);
  byteTx(6);
  byteTx(84);
  byteTx(6);
  byteTx(96);
  byteTx(6);

  // Start song
  byteTx(CmdSong);
  byteTx(START_SONG);
  byteTx(6);
  byteTx(69);
  byteTx(18);
  byteTx(72);
  byteTx(12);
  byteTx(74);
  byteTx(12);
  byteTx(72);
  byteTx(12);
  byteTx(69);
  byteTx(12);
  byteTx(77);
  byteTx(24);

  // Bump song
  byteTx(CmdSong);
  byteTx(BUMP_SONG);
  byteTx(2);
  byteTx(74);
  byteTx(12);
  byteTx(59);
  byteTx(24);

  // End song
  byteTx(CmdSong);
  byteTx(END_SONG);
  byteTx(5);
  byteTx(77);
  byteTx(18);
  byteTx(74);
  byteTx(12);
  byteTx(72);
  byteTx(12);
  byteTx(69);
  byteTx(12);
  byteTx(65);
  byteTx(24);
}





void rotate_CCW(int16_t angle_tmp, int16_t velocity)

{



drive(velocity, RadCCW);


while(angle<angle_tmp)

delayAndUpdateSensors(update_delay);


stop();


angle=angle-angle_tmp;/

}



void rotate_CW(int16_t angle_tmp, int16_t velocity)

{



drive(velocity, RadCW);


while((-angle)<angle_tmp)

delayAndUpdateSensors(update_delay);


stop();


angle=angle-angle_tmp;

}



void straight(int16_t distance_tmp, int16_t velocity)

{

drive(velocity, RadStraight);


while(distance<distance_tmp)

delayAndUpdateSensors(update_delay);


stop();


distance=distance-distance_tmp;

}



void stop(void)

{

drive(0, RadStraight);

delayAndUpdateSensors(update_delay);

}



void scan(unsigned int scan_thresh)

{





if (a2dConvert8bit(1) > scan_thresh)

{

if (scan_angle>300)

scan_angle-=10;

}

else

{

if (scan_angle<=1200)

scan_angle+=10;



}





servo_scan(scan_angle);

}


void delay_cycles(unsigned long int cycles)

{

while(cycles > 0)

cycles--;

}

void servo_scan(unsigned long int speed)

{

PORT_ON(PORTB, 1);

delay_cycles(speed);

PORT_OFF(PORTB, 1);

}





請問這段程式碼 哪裡是可以調整servo調整的地方呢?
哈哈哈哈,因為我是新手 這裡這我看別人的程式碼的
想說直接學習別人的程式碼看看(我是利用創建irobot機器人)
想我我都沒有修改程式碼 , 但是我的servo無法帶著轉動我的scanner!  
請問哪一段是可以修改的!




歡迎光臨 Robofun 機器人論壇 (https://robofun.net/forum/) Powered by Discuz! X3.2