/*goes forward for distance*/
void forward_for(int distance, int speed) {
set_create_distance(0);
while (get_create_distance() < distance ) {
create_drive_direct(speed,speed);
printf("value is %d", get_create_distance());
msleep(100);
}
create_drive_direct(0,0);
}
int go(y,x) //I don't quite get this part... seems like some braces missing
{
if (y>0)
{while (get_create_distance(5) < y)
{drive(x);}
}
if (y<0)
{while (get_create_distance(5) > y)
{drive(-x);}
}
}
/*Line follow: arguments are whatever threshold we are using,
plus however many iterations we want the function to perform*/
void line_follow(int threshold, int distance) {
while(get_create_distance() < distance) {
printf("Value: %d \n" , get_create_distance());
msleep(100);
if(analog(0)>=threshold) {
turn_right(3);
}
if(analog(0)<=threshold) {
turn_left(3);
}
}
}
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());
}
/*
* Class: cbccore_low_Create
* Method: get_create_distance
* Signature: ()I
*/
JNIEXPORT jint JNICALL Java_cbccore_low_Create_get_1create_1distance(JNIEnv *env, jobject obj, jfloat lag)
{
#ifdef CBC
return get_create_distance(lag);
#else
printf("Java_cbccore_low_Create_get_1create_1distance stub\n");
return 0;
#endif
}
/**
* move the iCreate, stop at a certain distance moved.
*/
int move(int left, int right, int distance)
{
int distance0 = 0;
int loop_done = 0;
distance0 = get_create_distance (.05);
create_drive_direct (left, right);
while (! loop_done)
{
printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
if (abs (distance0 - get_create_distance (.05)) > distance)
{
loop_done = 1;
}
msleep(50);
}
}
int main(int argc, char ** argv)
{
int i;
printf("Connecting...\n");
create_connect();
create_start();
printf("Spinning CW...n");
create_spin_CW(40);
// spin a while CW
for (i = 0; i < 10; ++i)
{
printf("Distance: %d Angle: %d\n", get_create_distance(), get_create_total_angle());
msleep(1000);
}
printf("Spinning CCW...n");
create_spin_CCW(40);
// spin a while CCW
for (i = 0; i < 10; ++i)
{
printf("Distance: %d Angle: %d\n", get_create_distance(), get_create_total_angle());
msleep(1000);
}
printf("Stopping...\n");
create_stop();
printf("Disconnecting...\n");
create_disconnect();
printf("Done.\n");
return 0;
}
void testDrive(){
float time1, time2;
set_create_distance(0);
printf("//Press Black Button to start/stop drive\n");
while(!black_button()); time1 = seconds();
create_drive_straight(-50);
printf("drive(50);\t");
sleep(2.0);
while(!black_button()); time2 = seconds();
stop();
printf("sleep(%f);\t//move %dmm",time2-time1,get_create_distance(0));
done = 1;
}
int main()
{
create_connect();
create_full();
float sec = seconds();
set_create_distance(0);
create_drive_distance(50, get_pot_reading(1, 1, 50), FORWARDS);
printf("distance traveled = %i, time = %f\n", get_create_distance(), seconds() - sec);
//sleep(2);
//create_spin_degrees(90, 300, LEFT);
//printf("%d\n", i);
create_disconnect();
return 0;
}
void main (int argc, char ** argv)
{
int loop_done = 0;
int task_A_done = 0;
int task_B_done = 0;
int angle = 0;
int dangle = 0;
int angle0 = 0;
int distance0 = 0;
int distance = 0 ;
int ddistance = 0;
// float lf_args[20];
// read inputs
// printf("Args: %d\n", argc);
// for (i = 2; i < argc; i++)
// {
// scanf("%lf%, argv[i]
// }
//get_create_total_ angle (.1);
//create_drive_direct (left speed, right speed)\
//move_to_position (motor #, motor power(-for backwards), end position)
//get_motor_done (motor#)
//clear_motor_position_counter (motor#)
//msleep (seconds)
//i'll probably use this often
create_connect ();
clear_motor_position_counter (0);
enable_servo (0);
set_servo_position (0 ,1300);
//set_servo_position (1800);
// ------------------------------------------------------------------------
//step:1 aim the arm to knock over the box
//raise arm
printf("-----Step 1-----\n");
move_to_position (0, -900, -1200);
while(! loop_done)
{
if (get_motor_done (0))
{
loop_done = 1;
}
msleep(100);
}
create_stop ();
loop_done = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (50,-50);
while(! loop_done)
{
printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done);
if (angle0 - get_create_total_angle (.05) >= 20)
{
loop_done = 1;
create_stop ();
}
msleep(100);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (-50,50);
while(! loop_done)
{
printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done);
if (angle0 - get_create_total_angle (.05) <= -20)
{
loop_done = 1;
create_stop ();
}
msleep(100);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
// ------------------------------------------------------------------------
//step:2 knock over box and turn
//lift arm
//turn 90
printf("-----Step 2-----\n");
move_to_position (0, -900, -3100);
msleep (900);
angle0 = get_create_total_angle (.05);
angle = angle0;
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
create_drive_direct (-178,-648);
while(! loop_done)
{
if (dangle >= 125)
{
create_drive_direct (-120, -181);
}
if (dangle >= 155)
{
task_A_done = 1;
create_stop ();
}
if (get_motor_done (0))
{
task_B_done = 1;
}
if (task_A_done && task_B_done)
{
loop_done = 1;
}
msleep(100);
angle = get_create_total_angle (.05);
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
// ------------------------------------------------------------------------
//step:4 aim claw to point of botguy
//turn 20
//lower arm
printf("-----Step 4-----\n");
angle0 = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (70, -70);
printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05));
//while(angle0 - get_create_total_angle (.05) < 4)
if (0)
{
printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05));
msleep(50);
}
move_to_position (0, 400, -900);
distance0 = get_create_distance (.05);
create_drive_direct (20, 20);
while (! loop_done)
{
if (get_create_distance (.05) - distance0 > 50)
{
loop_done = 1;
create_stop ();
}
}
set_servo_position (0 ,400);
loop_done = 0;
task_A_done = 0;
task_B_done = 0;
create_drive_straight (-20);
move_to_position (0, 600, -300);
while (! get_motor_done (0))
{
msleep (50);
}
create_drive_straight (-50);
msleep (2000);
//while (! loop_done)
if (0)
{
if (digital (10))
{
create_stop ();
loop_done = 1;
}
if (get_create_distance (.05) - distance0 < -100)
{
create_stop ();
loop_done = 1;
}
}
loop_done = 0;
// ------------------------------------------------------------------------
//step:5 grab botguy and lift him up (not complete)
printf("-----Step 5-----\n");
create_drive_direct (60, -60);
while(angle0 - get_create_total_angle (.05) < 3)
{
msleep(50);
}
create_stop ();
set_servo_position (0, 1510);
move_to_position (0, -1000, -3000);
msleep (5000);
// ------------------------------------------------------------------------
//step:6 move backwards untill the bumper hits the transport (not completed)
printf("-----Step 6-----\n");
create_drive_direct (130, 280);
distance0 = get_create_distance (.05);
while (! loop_done)
{
printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
if (get_create_distance (.05) - distance0 > 700)
{
loop_done = 1;
}
msleep(50);
}
loop_done = 0;
move_to_position (0, -900, -500);
create_drive_straight (-100);
msleep (1000);
set_servo_position (0, 200);
create_disconnect ();
printf("-----Step 6-B-----\n");
create_drive_direct (-400, -150);
distance0 = get_create_distance (.05);
while(! loop_done)
{
printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
if (get_create_distance (.05) - distance0 < -100)
{
loop_done = 1;
create_stop ();
}
msleep(50);
}
loop_done = 0;
// ------------------------------------------------------------------------
//step:7 drop botguy.
printf("-----Step 7-----\n");
move_to_position (0, -900, -300);
msleep (6000);
set_servo_position (0, 200);
create_disconnect ();
}
