void drive_random()
{
srand ( time(NULL) );
int direction=rand()%361-180; //-180 to +180
if(direction<0)
{
set_create_total_angle(0);
create_drive_direct(-200,200);
while(get_create_total_angle(0.1)>direction);
}
else
{
set_create_total_angle(0);
create_drive_direct(200,-200);
while(get_create_total_angle(0.1)<direction);
}
int i=0;
while(i<1000)
{
if(get_create_lbump(0.1)||get_create_rbump(0.1)) //if bumper pressed
{
i=2000;
}
else
{
create_drive_direct(300,300);
}
i++;
msleep(1);
}
create_stop();
}
int main()
{
X=0;
create_connect();
set_create_total_angle(0);
printf("X\tAngle");
while(black_button()==0)
{
if (a_button()==1)
{
turnHalfCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (b_button()==1)
{
turnHalfCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (left_button()==1)
{
TurnCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (right_button() == 1)
{
TurnCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
}
}
void testArc(){
float time1, time2;
cbc_display_clear();
set_create_total_angle(0);
printf("//Press Black Button to start/stop arc\n");
while(black_button()) while(!black_button()){}
time1 = seconds();
create_drive(-100,-580);
while(!black_button()); time2 = seconds(); stop();
printf("%fsecs\t %dmm\n",time2-time1,get_create_distance(0));
while(!black_button());
clawSB();
armPosSB();
printf("\npress A for too far, B for not far enough, Black for just right\n");
while(!done){
if(a_button()){
arc_value = arc_value-5;
done = 1;
}
if(b_button()){
arc_value = arc_value+5;
done = 1;
}
if(black_button()) done = 1;
}
printf("arc = %d",arc_value);
printf("press black button to exit");
while(!black_button());
}
void Start() //Misc. set up functions
{
create_connect();
set_create_total_angle(0);
//wait_for_light(LightPort);
shut_down_in(RunTime); //TEST THIS
}
void Start (void)
{
wait_for_light(LightPort);
shut_down_in (117); //TEST THIS
create_connect();
set_create_total_angle(0);
}
/*
* Class: cbccore_low_Create
* Method: set_create_total_angle
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_cbccore_low_Create_set_1create_1total_1angle(JNIEnv *env, jobject obj, jint angle)
{
#ifdef CBC
set_create_total_angle(angle);
#else
printf("Java_cbccore_low_Create_set_1create_1total_1angle stub\n");
#endif
}
int init()// move the grabber motors to the start position.
{
motor(1,-100); // move motor 1 at -100%
motor(3,-100); // move motor 3 at -100%
msleep(500); // stop moving the motors after .5 seconds
set_create_distance(0); // initializes the distance accumulation to start at 0
set_create_total_angle (0); // initializes the angle accumulation to start at 0
ao(); // stop all motors
return 0;
}
int main()
{
create_connect();
enable_servos();
set_create_total_angle(0);
set_create_distance(0);
WhichSide();
CenterCamera();
Lift();
}
int main()
{
X=0;
create_connect();
set_create_total_angle(0);
printf("X\tAngle");
cbc_display_clear();
printf("A to turn CCW 90deg.\tB to turn CW 90deg.\n");
printf("Left to turn CCW 45deg.\tRight to turn CW 45deg.\n");
printf("Down to force angle readjustment.\n");
while(black_button()==0)
{
if (a_button()==1)
{
turn(tspeed,45);
//turnHalfCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (b_button()==1)
{
turn(tspeed,-45);
//turnHalfCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (left_button()==1)
{
turn(tspeed,90);
//turnCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (right_button() == 1)
{
turn(tspeed,-90);
//turnCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (down_button() == 1)
{
turnAdjust();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
}
}
void turn_for(int angle, int speed) {
set_create_total_angle(0);
if (speed > 0) {
while (get_create_total_angle() >-angle) {
create_spin_CW(speed);
}
create_spin_CW(0);
}
else if (speed < 0) {
while (get_create_total_angle() < angle) {
create_spin_CW(speed);
}
create_spin_CW(0);
}
}
void main(){
int X = 0;
create_connect();
set_create_distance(0);
set_create_total_angle(0);
enable_servos();
armPosBlockStack1();
clawStack1();
/* create_drive(-500,-380);
sleep(2.3);
create_stop();
}
*/
// writeToFile();
printf("%f",seconds());
while(1) //black_button
{
if (a_button()){
testDrive();
}
if (b_button()){
testDetectPVC();
}
if (up_button()){
}
if (down_button()){
testTurnAdjust();
}
if (left_button()){
testTurnCCW();
}
if (right_button()){
testTurnCW();
}
}
}
int main()
{
set_each_analog_state(0,0,0,1,0,0,0,0);// This line sets analog ports to be pullup (0) or floating (1)
create_connect();
enable_servos();
set_create_total_angle(0);
set_create_distance(0);
//AUO=start_process(ArmUpOpen);
ArmUp();
set_servo_position(
sleep(3);
CenterCamera();
create_drive_straight(-200);
while(digital(8) != 1)
{}
create_stop();
sleep(1);
ArmDownClose();
sleep(3);
beep();
create_drive_straight(200);
sleep(2);
create_stop();
}
int main()
{
printf("initializing. please wait.\n");
enable_servos();
sleep(.25);
mav(3,100);
sleep(.8);
off(3);
enableServoDown();
sleep(.5);
release();
set_servo_position(1,120); // mini servo down
mav(3, 400); // move arm to open position -- test values -- should be slightly angeled forwards
sleep(1);
off(3);
printf("servos initalized.\n");
create_connect();
create_full();
set_create_total_angle(0);
printf("create initialized.\n");
benDance();
}
//main stuff haha
int main()
{
int claw = 1; //claw port
int arm = 0; //arm port
create_connect();
printf("create connected");
set_create_total_angle(0);
//create setup
turn_for(10, -200); //Turns the create around
//does some important stuff
create_drive_direct(-100, -100);
set_servo_position(claw, 1561);
enable_servos();
msleep(300);
set_servo_position(arm, 1850); //arm down
msleep(500);
set_servo_position(claw, 1000);
msleep(500);
set_servo_position(arm, 1643); //grabs ball
msleep(500);
turn_for(36, 200); //spins onto first black path to line follow later
msleep(500);
forward_for(50, -100); //moves forward onto the black line
line_follow(blackThresh , 212); //starts following the black line
set_servo_position(claw, 1561); //drops ball
//dropped ball already
msleep(400);
forward_for(500, 100); //back it up back it up
set_servo_position(arm, 245); //holds the arm up
msleep(100);
set_create_distance(0);
set_create_total_angle(0);
while (get_create_total_angle() > -10) { //spins itself onto the angle of other black line
create_spin_CW(200);
}
create_spin_CW(0); //stops spinning
msleep(1500);
create_drive_direct(-200, -200); //goes onto the black line to line follow later
msleep(1000);
create_drive_direct(0,0); //stops itself
msleep(500);
set_create_total_angle(0);
while (get_create_total_angle() > -3) { //angles itself a little bit
create_spin_CW(200);
}
create_spin_CW(0); //stops spinning
msleep(400);
create_drive_direct(-200, -200); //Drives forward a little bit to the next black line
msleep(1500);
create_drive_direct(0,0);
set_create_total_angle(0);
while (get_create_total_angle() > -7) { //Angles itself so it can drive in the middle
create_spin_CW(200);
}
create_spin_CW(0); //stops spinning
create_drive_direct(-230,-230); //Drives itself into the middle lane
msleep(4000);
create_drive_direct(0,0); //Reset stuff
set_create_total_angle(0);
while(analog(0) < blackThresh) { //Align itself using line following in the middle lane
create_spin_CW(50);
printf("Value is %d \n", analog(0));
if(digital(0) >= blackThresh) {
printf("I have found the middle black line!");
create_spin_CW(1); //Angle itself so it is exactly on the black line
}
}
create_spin_CW(0); //resets something for some reason why is this here?
set_servo_position(claw, 1561); //grabs the red cube
msleep(200);
set_servo_position(arm, 1984);
msleep(500);
set_servo_position(claw, 1144);
msleep(500);
return 0;
}
void CenterCamera() //CAMERA MUST BE AT CENTER OF CREATE, POINTING Right RESPECT TO CREATE
{
track_update();
if (track_x (0,0) > 81)
{
create_drive_straight(-200);
while (track_x (0,0) > 81) //WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY
{
track_update();
}
create_stop();
drivemm(inchestomm(4.9), 1);
set_create_total_angle(0);
create_drive(101.3,1); //uses int. does it round?
sleep(2.057252314);
create_stop();
get_create_total_angle(.1);
printf("asdb");
if (get_create_total_angle(.1) != 90)
{
set_create_total_angle(get_create_total_angle(.1) - 90);
printf("%f\n",get_create_total_angle(.1));
if (get_create_total_angle(.1) > 0) //CCW ADDS TO TOTAL ANGLE
{
create_spin_CW(100);
while (get_create_total_angle(.1) < 0);
{
printf("aeqweqw");
}
create_stop();
}
if (get_create_total_angle(.1) < 0)
{
create_spin_CCW(100);
while (get_create_total_angle(.1) > 0);
{
printf("BBBB");
}
create_stop();
}
}
}
/* (track_x (0,0) < 79)
{
create_drive_straight(200);
while (track_x (0,0) < 79)
{
track_update();
}
create_stop();
beep();
set_create_distance(0);
drivemm(-inchestomm(5), 1);
create_drive(206.972051,1);
sleep(1);
create_stop();
create_drive_straight(-200);
while(digital(8 == 0))
{
}
create_stop();
}*/
}
int main()
{
int stupid_int = 0;
int value = 0;
int i = 0;
int angle_of_pole = 90;
camera_open(LOW_RES);
camera_update();
create_connect();
set_create_total_angle(0);
value = get_create_total_angle();
printf("Angle is %d\n", value);
//------------------------
// part 1: spin until past all booster sections
while(get_create_total_angle() > -85){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(1,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
enable_servos();
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of_pole = 60;
//end part 3
//part 4: repeat everything
/*while(get_create_total_angle() > -80){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 50;
//end part 3
while(get_create_total_angle() > -70){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 50;
//end part 3
while(get_create_total_angle() > -100){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 40;
//end part 3
while(get_create_total_angle() > -60){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 60;
//end part 3*/
//end part 4
}
int reseta()
{
set_create_total_angle(0);
}
