Nexus Automation Nexus Robot, Руководство пользователя

Страница: 76 / 128

www.nexusrobot.com Robot Kits manual
72
unsigned char irL0_pin=0; // Set Analog pin 0 as the left Infrared Sensor signal pin
unsigned char irC0_pin=1;
unsigned char irR0_pin=2; // Set Analog pin 2 as the right Infrared Sensor signal pin
int ir_distance(unsigned char ir) {
int val=analogRead(ir); // Read the data from the Infrared Sensors
return (6762/(val-9))-4; // Change the data
}
/*********************************************/
// bumper
unsigned char bumperL_pin=12; // Set pin 12 as the left Bumper’s Sensor signal pin
unsigned char bumperC_pin=3; // Set pin 3 as the front Bumper’s Sensor signal pin
unsigned char bumperR_pin=2; // Set pin 2 as the right Bumper’s Sensor signal pin
/ *********************************************/
irqISR(irq1,isr1); // Intterrupt function.on the basis of the pulse, work for wheel1
MotorWheel wheel1(9,8,4,5,&irq1,REDUCTION_RATIO,int(144*PI));
//This will create a MotorWheel object called Wheel1
//Motor PWM:Pin9, DIR:Pin8, Encoder A:Pin4, B:Pin5
irqISR(irq2,isr2);
MotorWheel wheel2(10,11,6,7,&irq2,REDUCTION_RATIO,int(144*PI));
R2WD _2WD(&wheel1,&wheel2,WHEELSPAN);
unsigned int speedMMPS=80;
void setup() {
//TCCR0B=TCCR0B&0xf8|0x01; // warning!! it will change millis()
TCCR1B=TCCR1B&0xf8|0x01; // Pin9,Pin10 PWM 31250Hz
//TCCR2B=TCCR2B&0xf8|0x01; // Pin3,Pin11 PWM 31250Hz
SONAR::init(13); // Pin13 as RW Control
_2WD.PIDEnable(0.26,0.02,0,10); // Enable PID
}