void right_angle(int dir){
if(dir==right){
rightC(0.53,120);
}else{
leftC(0.53, 120);
}
}
void right_angle2(int dir){
if(dir==right){
stop(0.1);
rightC(0.57,120);
}else{
stop(0.1);
leftC(0.60, 120);
}
}
/*
A high-level serial passthrough implementation. Used by cli to start an
arbitrary serial passthrough "proxy". Optional callbacks can be given to allow
for specialized data processing.
*/
void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer
*leftC, serialConsumer *rightC)
{
waitForSerialPortToFinishTransmitting(left);
waitForSerialPortToFinishTransmitting(right);
if (!leftC)
leftC = &nopConsumer;
if (!rightC)
rightC = &nopConsumer;
LED0_OFF;
LED1_OFF;
// Either port might be open in a mode other than MODE_RXTX. We rely on
// serialRxBytesWaiting() to do the right thing for a TX only port. No
// special handling is necessary OR performed.
while(1) {
// TODO: maintain a timestamp of last data received. Use this to
// implement a guard interval and check for `+++` as an escape sequence
// to return to CLI command mode.
// https://en.wikipedia.org/wiki/Escape_sequence#Modem_control
if (serialRxBytesWaiting(left)) {
LED0_ON;
uint8_t c = serialRead(left);
serialWrite(right, c);
leftC(c);
LED0_OFF;
}
if (serialRxBytesWaiting(right)) {
LED0_ON;
uint8_t c = serialRead(right);
serialWrite(left, c);
rightC(c);
LED0_OFF;
}
}
}
int main()
{
printf("version 1.9.5\n");
shut_down_in(0.010*1000);
int start=seconds();//starting time for two minutes
enable_servo(0);
set_servo_position(0,2047);
straight(1.8, 170);//gets in front of transport
rightC(0.28, 100);
leftC(0.28, -100);
straight(1.65, 170);
leftC(0.44, 100);
printf("completed first dead reckoning\n");
set_analog_pullup(ETport, 0);
//until distance is 5 centimeter
/*
int claw_threshold = 13;
while(analog10(ETport)>claw_threshold){
back(0.1, -10);
msleep(1000);
printf("move once\n");
printf("%i\n", analog10(ETport));
}*/
back(0.25, -20);//pushes against transport
msleep(2000);
set_servo_position(0, 500);//puts claw on transport
msleep(1000);
printf("done with attaching to transport\n");
while(digital(8)==0)//goes forward with transport until it hits the wall
{
straight(0.1, 100);
msleep(10);
}
printf("at wall\n");
back(0.25, -50);//backs up from wall to deposit transport
set_servo_position(0, 2047);//raises claw
msleep(1500);
while(digital(8)==0){//goes forward until it hits wall for second time
straight(0.1, 100);
msleep(10);
}
printf("at wall 2\n");
back(0.35, -100);//goes back and turns left in order to be oriented with left inner wall
leftC(0.6, 142);
int bump_counter=0;
while(digital(8)==0){//hits side wall
straight(0.1, 100);
msleep(10);
}
bump_counter++;//has one bump
int i=0;
while(i<10){//if hits side wall, pause
stop(0.2);
i++;
}
back(0.25, -100);
leftC(0.46, 100);
camera_open(LOW_RES);
armUp();
clawOpen();
double start_wait=seconds();
while((seconds()-start_wait)<=25){//wait for poms or for 25 seconds
int i=0;
while(i<10){//picks latest image from the buffer
camera_update();
i++;
}
if(get_object_count(chan)>0){//if camera sees objects, skip the 25 seconds and go onto picking up stuff
break;
}else{//if camera doesn't see any objects, keep waiting until 25 seconds is up
stop(0.1);
}
}
int x=get_object_center(chan, 0).x;//declares global unchanging variable for the x location of the largest object
while((seconds()-start)<=120){//while there is still time left
int area=get_object_area(chan, 0);//creates local changing variable. this is th area of the largest object camera sees
if(area<=600){//if the object is small enough(far enough), navigate towards object. 600 is threshold
int i=0;
while(i<10){//buffer updating
camera_update();
i++;
}
navigate_poms(x);//similar to line followig function; gets to poms.
}
armDown();
clawClose();
armUp();
/*
leftF(0.5, 100, 80);//turn and go forward until transport is contacted
while(digital(8)==0){
straight(0.1, 100);*/
}
/*
back(1.5, 100);
leftC(0.44, 100);
while(digital(8)==0 && digital(9)==1){
straight(0.1, 100);
msleep(10);
if(digital(9)==0){
rightF(0.1, 100, 40);
left(0.1, 100);
msleep(10);
}
}
straight(2, 200);
rightC(0.44, 100);
straight(1, 80);
rightC(0.92, 100);
while(analog10(ETport<=600)){
msleep(10);
}
msleep(10000);
straight(1, 80);
leftC(0.44, 100);
straight(2, 200);
*/
return 0;
}
int main()
{
//wait_for_light(1);//initialization
printf("version 1.9.5\n");
shut_down_in(115);
int servo_counter=0;
while(servo_counter<=3){
enable_servo(servo_counter);
servo_counter++;
}
// initialize routine
set_servo_position(0,2047);
armUp();
clawClose();
//get in front of transport
straight(1.8, 170);
rightC(0.28, 100);
leftC(0.28, -100);
straight(1.65, 170);
leftC(0.44, 100);
printf("completed first dead reckoning\n");
//until distance is 5 centimeter
/*
int claw_threshold = 13;
while(analog10(ETport)>claw_threshold){
back(0.1, -10);
msleep(1000);
printf("move once\n");
printf("%i\n", analog10(ETport));
}*/
back(0.25, -20);//pushes against transport
msleep(2000);
set_servo_position(0, 500);//puts claw on transport
msleep(1000);
printf("done with attaching to transport\n");
while(digital(8)==0)//goes forward with transport until it hits the wall
{
straight(0.1, 100);
msleep(10);
}
printf("at wall\n");
back(0.15, -50);//backs up from wall to deposit transport
set_servo_position(0, 2047);//raises claw
msleep(1500);
while(digital(8)==0){//goes forward until it hits wall for second time
straight(0.1, 100);
msleep(10);
}
printf("at wall 2\n");
back(0.35, -100);//goes back and turns left in order to be oriented with left inner wall
leftC(0.6, 142);
int bump_counter=0;
while(digital(8)==0){//hits side wall
straight(0.1, 100);
msleep(10);
}
bump_counter++;//has one bump
int i=0;
while(i<10){//if hits side wall, pause
stop(0.2);
i++;
}
/*
==========================
==========End of routine 1======
KIPR at the wall
==========================
*/
// Pause here to wait for Create to drop POMS
msleep(1*1000);
//
int start=seconds();// start time
printf("will get out of corner\n");
back(0.5, -100);
while(seconds()-start<=3){//separates back from right angle
stop(0.1);
}
right_angle(left);
while(seconds()-start<=5){//separates right angle from straight
stop(0.1);
}
double start_drive=seconds();
straight(0.7, 100);//go to poms
stop(0.5);
camera_open(LOW_RES);
/*while(seconds()-start<=20){
int update_counter=0;
while(update_counter<10){
camera_update();
update_counter++;
}
if(get_object_count(chan)==0){
stop(0.1);
}
int x=get_object_center(chan, 0).x;
int area=get_object_area(chan, 0);
if(area<=400){
navigate(x);
}
if(area>=800){
//if objects are clumped are clumped, area will be automatically larger.
//this means that KIPR will start earlier than it should. We counter this here
//straight(0.18, 100);
straight(0.1, 100);
break;
}
}*/
stop(0.1);
printf("object seen\n");
double end_drive=seconds();//only for simulation. real end and start times would have to be taken from whole routine
//pick up POMS that are right there
pickup();
int offset=(end_drive-start_drive)*(3.0/4.0);;//half of distance from back of launch area to border of launch area
back((end_drive-start_drive)+1.5, -100);//go to the back of wall plus a little more to straighten out
printf("at back wall\n");
straight(2.0, 100);//goes halway between border and back of launch area
right_angle(left);//faces transport
int starpof=seconds();//time after right angle
while(seconds()-starpof<=2){
motor(motorL, 0);
motor(motorR, 0);
}
while(digital(8)==0){
straight(0.1, 100);
}
straight(0.1, 100);
printf("touching transport\n");
motor(motorL, 0);
motor(motorR, 0);
dropoff();//drops poms in launch area
printf("poms in transport\n");
back(0.25, -100);
int starfop=seconds();
while(seconds()-starfop<=2){
motor(motorL, 0);
motor(motorR, 0);
}
int turns=1;
while(turns<=2){
right_angle(right);
stop(0.3);
turns++;
}
/*
======================
======Part 2 of routine 2====
======================
*/
while(1){
stop(0.5);//
while(digital(8)==0){
straight(0.1, 100);
}
back(0.3, -100);
right_angle(left);
pickup();
back((end_drive-start_drive)+1.5, -100);
straight(2.2, 100);
right_angle(left);
while(digital(8)==0){
straight(0.1, 100);
}
dropoff();
back(0.5, -100);
right_angle(left);
right_angle(left);
}
/*while(1){
start=0;
while(digital(8)==0){
straight(0.1, 80);
}
back(0.5, -100);
right_angle(right);
straight(0.5, 100);
right_angle(right);
right_angle(right);
while(seconds()-start<=20){
int update_counter=0;
while(update_counter<10){
camera_update();
update_counter++;
}
if(get_object_count(chan)==0){
stop(0.1);
}
int x=get_object_center(chan, 0).x;
int area=get_object_area(chan, 0);
if(area<=400){
navigate(x);
}
if(area>=800){
//if objects are clumped are clumped, area will be automatically larger.
//this means that KIPR will start earlier than it should. We counter this here
//straight(0.18, 100);
straight(0.1, 100);
break;
}
}
int servo_counter=1;
while(servo_counter<=3){
enable_servo(servo_counter);
printf("servo %d enabled\n", servo_counter);
servo_counter++;
}
pickup();
int offset=(end_drive-start_drive)*(3.0/4.0);;//half of distance from back of launch area to border of launch area
back((end_drive-start_drive)+1.5, -100);//go to the back of wall plus a little more to straighten out
printf("at back wall\n");
straight(1.8, 100);//goes halway between border and back of launch area
right_angle(left);//faces transport
int starpof=seconds();//time after right angle
while(seconds()-starpof<=2){
motor(motorL, 0);
motor(motorR, 0);
}
while(digital(8)==0){
straight(0.1, 100);
}
straight(0.1, 100);
printf("touching transport\n");
motor(motorL, 0);
motor(motorR, 0);
dropoff();//drops poms in launch area
printf("poms in transport\n");
back(0.25, -100);
int starfop=seconds();
while(seconds()-starfop<=2){
motor(motorL, 0);
motor(motorR, 0);
}
int turns=1;
while(turns<=2){
right_angle(right);
turns++;
}
repeat_runs++;
}*/
while(1){
int servo_counter=0;//enables servos
while(servo_counter<=3){
enable_servo(servo_counter);
servo_counter++;
}
armUp();//raises arm at start
stop(0.5);
while(digital(TOUCH_SENSOR)==0){//go forward until left wall is contacted
straight(0.1, 100);
}
stop(0.1);
back(0.3, -100);//back up
stop(0.1);
right_angle2(left);//face poms
stop(0.1);
back(3+1.5, -100);//back up to rocket wall to straighten out
stop(0.1);
straight(3+1.5, 100);//go to poms
stop(0.1);
pickup();//pick poms up
stop(0.1);
back(3+1.5, -100);// back up until rocket wall
stop(0.1);
straight(2.2, 100);//go forward
stop(0.1);
right_angle2(left);//turn to face transport
stop(0.1);
while(digital(TOUCH_SENSOR)==0){//go forward until transport contacted
straight(0.1, 100);
}
stop(0.1);
dropoff();//drop poms off
stop(0.1);
back(0.5, -100);//back up from transpor
stop(0.1);
right_angle2(right);//orient for next iteration
stop(0.1);
right_angle2(right);
}
return 0;
}
