|
grendel
Rookie
Joined: Mon Feb 18, 2013 6:16 am Posts: 5
|
 Re: Aligning the Robot Based on IR Beacon
here is some code:
#pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, HTServo) #pragma config(Sensor, S2, HTIRS2, sensorI2CCustom) #pragma config(Motor, mtr_S1_C1_1, OmniFL, tmotorTetrix, PIDControl, reversed, encoder) #pragma config(Motor, mtr_S1_C1_2, OmniFR, tmotorTetrix, PIDControl, encoder) #pragma config(Motor, mtr_S1_C2_1, OmniRR, tmotorTetrix, PIDControl, encoder) #pragma config(Motor, mtr_S1_C2_2, OmniRL, tmotorTetrix, PIDControl, reversed, encoder) #pragma config(Motor, mtr_S1_C3_1, ShoulderL, tmotorTetrix, PIDControl, encoder) #pragma config(Motor, mtr_S1_C3_2, ShoulderR, tmotorTetrix, PIDControl, reversed, encoder) #pragma config(Servo, srvo_S1_C4_1, Slide, tServoContinuousRotation) #pragma config(Servo, srvo_S1_C4_2, Wrist, tServoStandard) #pragma config(Servo, srvo_S1_C4_3, Seeker, tServoStandard) #pragma config(Servo, srvo_S1_C4_4, EmptyServo, tServoStandard) #pragma config(Servo, srvo_S1_C4_5, EmptyServo, tServoStandard) #pragma config(Servo, srvo_S1_C4_6, EmptyServo, tServoStandard) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "drivers/hitechnic-irseeker-v2.h"
task main() { int _dirAC = 0; int acS1, acS2, acS3, acS4, acS5 = 0; int _dirEnh, _strEnh; int _speed = 0; int _level = 0; int _forwardcount=0; bool _aligned = false; bool _done = false;
_speed = 30; motor[OmniFL] = _speed; motor[OmniFR] = _speed; motor[OmniRL] = _speed; motor[OmniRR] = _speed; wait1Msec(1000);
while(!_done) {
_dirAC = HTIRS2readACDir(HTIRS2); if (_dirAC < 0) break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 )) break; // I2C read error occurred
if (!HTIRS2readEnhanced(HTIRS2, _dirEnh, _strEnh)) break; // I2C read error occurred
if(_dirAC>0) { _speed = 40; if(_dirAC > 5 || acS4 > 0 ) { if(_aligned) _speed=30; // Strafe right // motor[OmniRR] = _speed; //reversed// motor[OmniRL] = -1*_speed; motor[OmniFL] = _speed; motor[OmniFR] = -1*_speed; //reversed// } else if((_dirAC < 5) || (acS2 > 0)) { // Strafe left // if(_aligned) _speed=30; motor[OmniRR] = -1*_speed; //reversed// motor[OmniRL] = _speed; motor[OmniFL] = -1*_speed; motor[OmniFR] = _speed; //reversed// } else {
motor[OmniFL] = 0; motor[OmniFR] = 0; motor[OmniRL] = 0; motor[OmniRR] = 0;
if(!_aligned) // or !aligned { _aligned=true;
if(_strEnh < 85) _level=3; else if(_strEnh < 170) _level=2; else _level=1; // move arm to the correct height and then move forward // _level 1 = lowest rung; _level 2 = middle rung; _level 3 = highest rung // insert code here }
_speed=30; // _speed is set to 25; adjust motor speed motor[OmniFL] = _speed; motor[OmniFR] = _speed; motor[OmniRL] = _speed; motor[OmniRR] = _speed;
_forwardcount++;
if(_forwardcount>40) //try again/// do you have the code to adjust the arms? Yes { _done=true; // turn on boolean switch to exit the while loop // back up from rack motor[OmniFL] = -1*_speed; motor[OmniFR] = -1*_speed; motor[OmniRL] = -1*_speed; motor[OmniRR] = -1*_speed; wait1Msec(500); _speed = 0; } } } } // _speed is set to 0; turn off motors motor[OmniFL] = _speed; motor[OmniFR] = _speed; motor[OmniRL] = _speed; motor[OmniRR] = _speed;
}
|