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/)