void networkSetup()
{
//jumper digital 8-11 for colors red-blue
char myip[16];
if(digital(8)) strcpy(myip,REDIP);
else if(digital(9)) strcpy(myip,YELLOWIP);
else if(digital(10)) strcpy(myip,GREENIP);
else if(digital(11)) strcpy(myip,BLUEIP);
else {
printf("not jumpered correctly!\nexiting program!\n");
exit(0);
}
char string[50]="ifconfig rausb0 ";
strcat(string,myip);
system("iwconfig rausb0 essid BotGuy");
system("ifconfig rausb0 key open");
system("dhclient rausb0");
system(string);
system("ifconfig");
cbc_display_clear();
printf("connected to network\n");
}
int main()
{
while(1) {
//If left bumper is hit (or if both sensors are hit)
if(digital(lSensor)) {
//Backup
bk(lMotor); bk(rMotor);
sleep(backupTime);
//Move Right
moveRight(velocity);
sleep(turnTime);
}
//If just right bumper is hit
else if(digital(rSensor)) {
//Backup
bk(lMotor); bk(rMotor);
sleep(backupTime);
//Move Left
moveLeft(velocity);
sleep(turnTime);
}
//No Sensor is Hit
else {
//Move Forward
moveForward(velocity);
}
}
}
void claw_down(){
motor(Motor_Up,Motor_down_speed);
while(!digital(Sensor_Down)){}
freeze(Motor_Up);
//wait so it doesn't f**k up
msleep(500);
//up for straffes seil
motor(Motor_Up,Motor_up_speed);
while(!digital(Sensor_Down)){}
freeze(Motor_Up);
}
// TEST:
void test(){
ao();
display_clear();
printf("test now");
while(digital(TSS) == 0);
msleep(1000);
}
void release() { // open claw until touch sensor #14 is activated
mav(3, -400);
while(digital(14) ==0) {
}
off(3);
clear_motor_position_counter(3);
}
// prepare:
void prepare(){
camera_open(LOW_RES);
camera_load_config(CONFIG);
camera_update();
camera_update();
camera_update();
// prepare camera
enable_servos();
set_servo_position(BSV, BSV_LEVEL);
set_servo_position(ASV, ASV_BACK);
set_servo_position(RSV, RSV_DOWN);
set_servo_position(SSV, SSV_BACK);
// prepare servos
set_analog_pullup(FSS, 0);
set_analog_pullup(BSS, 0);
// prepare sensors
display_clear();
printf("prepare succeeds");
while(digital(TSS) == 0);
msleep(1500);
//wait_for_light(SSS);
//shut_down_in(TIME_LIMIT);
// show controller that coke is ready
}
void FORTE_Digital::executeEvent(int pa_nEIID){
QO() = QI();
STATUS() = 0;
switch (pa_nEIID){
case scm_nEventINITID:
if(true == QI()){
libcbc_init(); //TODO: only call once //initialises the CBC Library :)
if(PORT() >= 8 && PORT() <= 15){ //digital PORTS!!
m_nPort = PORT();
}
}
else{
QO() = false;
STATUS() = 1;
}
sendOutputEvent(scm_nEventINITOID);
break;
case scm_nEventREQID:
if(true == QI()){
STATUS() = digital(m_nPort);
}
sendOutputEvent(scm_nEventCNFID);
break;
}
}
int main()
{
while(1)
{
int isPressed = digital(15);
if(isPressed == 1)
{
move_at_velocity(0,-600);
move_at_velocity(3,0);
msleep(3000);
move_at_velocity(0,-400);
move_at_velocity(3,-400);
msleep(3000);
move_at_velocity(0,0);
move_at_velocity(3,-500);
msleep(3000);
} else {
moveTowardsLight();
}
}
return;
}
void main()
{
calibrate();
enable_servos();
set_digital_pullup(BUTTON,1);
//set_servo_position(SERVO,800);
int on=1;
int intervall = 5;
while(!b_button()){
if(digital(BUTTON)){
stop();
msleep(400);
printf("button pressed");
on=(on+1)%2;
}
if(on){
drive_speed=analog10(SPEED_SENSOR)/10;
if(drive_speed>100){
drive_speed=100;
}
forward();
while(analog10(LEFT_SENSOR)>=left_blk-150){
turn_left() ;
set_servo_position(SERVO,servo_pos);
if(digital(BUTTON)){ on=(on+1)%2;msleep(100);}
if(servo_pos>=400||servo_pos<=0){intervall*=-1; }
servo_pos=servo_pos+intervall;
}
while(analog10(RIGHT_SENSOR)>=right_blk-150){
turn_right();
set_servo_position(SERVO,servo_pos);
if(digital(BUTTON)){ on=(on+1)%2;msleep(100);}
if(servo_pos>=400||servo_pos<=0){intervall*=-1; }
servo_pos=servo_pos+intervall;
}
set_servo_position(SERVO,servo_pos);
if(servo_pos>=400||servo_pos<=0){intervall*=-1; }
servo_pos=servo_pos+intervall;
}
else{
stop();
}
}
stop();
}
void touch_back(int sen)
{
while(digital(TOUCH_SEN) == 0)
{
bk(MOT_RIGHT);
bk(MOT_LEFT);
}
}
void ReadEncoders()
{
int i;
cLLast = cL;
cRLast = cR;
cL = read_encoder(0);
cR = read_encoder(1);
/* Stalled motors */
if (cL == cLLast && cR == cRLast)
{
if (!fStalling)
msStall = mseconds() + 250L;
fStalling = 1;
}
else
fStalling = 0;
if (fStalling && mseconds() >= msStall)
{
printf("Stall!\n");
beep();beep();
fStalled = 1;
}
/* Read bump sensors and do block detection */
fBlocked = 0;
for (i = 0; i < 4; i++)
{
rgfBump[i] = digital(i+12);
if (rgfBump[i] && sgn(rgpwr[rgibumpMotor[i]]) == rgibumpSgn[i])
{
fBlocked = 1;
rgfBlock[i] = 1;
}
else
rgfBlock[i] = 0;
}
if (digital(10))
fBall = 1;
}
void calibrate(){
printf("Please press the claibrate Button");
set_b_button_text("calibrate");
while(!b_button()){}
display_clear();
printf("calibrating");
//claws down
motor(Motor_Up,Motor_down_speed);
while(!digital(Sensor_Down)){}
freeze(Motor_Up);
//wait so it doesn't f**k up
msleep(500);
//up for straffes seil
motor(Motor_Up,Motor_up_speed);
while(digital(Sensor_Down)){}
freeze(Motor_Up);
claw_close();
}
int main()
{
create_connect();
create_full();
while(digital(15) == 0);
create_drive_direct(-1500, -1500);
while(!(a_button_clicked()));
return 0;
}
void calibrate(){
printf("Bitte setze alle Sensoren auf den schwarzen Streifen\n");
set_a_button_text("calibrate");
while(!digital(BUTTON)){}
right_blk = analog10(RIGHT_SENSOR);
left_blk = analog10(LEFT_SENSOR);
msleep(500);
printf("left ");
printf("%i",left_blk);
printf("\n");
printf("right ");
printf("%i",right_blk);
printf("\n");
set_a_button_text("start");
while(!digital(BUTTON)){}
set_a_button_text("A");
set_b_button_text("stop");
}
int main()
{
printf("starting connor\n");
enable_servo(1);
set_servo_position(1,100);
//disable_servo(1)
create_connect();
while (! digital(11))
{
printf("%d\n", digital(11));
create_drive_direct(-150,-150);
}
printf("starting connor\n");
set_servo_position(1,1400);
while (get_create_lbump() == 0 && get_create_rbump() == 0)
{
create_drive_direct(150, 150); // drive straight at 150mm/s
}
disable_servos();
}
void moveBack1(){
int frontDis = analog_et(FSS);
int backDis = analog_et(BSS);
int errorFront = FSS_BACK_DIS - frontDis;
int errorBack = BSS_BACK_DIS - backDis;
int turnLM = MOVE_BACK_TP + errorFront * FSS_BACK_Kp + errorBack * BSS_BACK_Kp;
int turnRM = MOVE_BACK_TP - errorFront * FSS_BACK_Kp - errorBack * BSS_BACK_Kp;
while(digital(TSS) == 0 && backDis > 280){
motor(LM, turnLM);
motor(RM, turnRM - 4);
frontDis = analog_et(FSS);
backDis = analog_et(BSS);
errorFront = FSS_BACK_DIS - frontDis;
errorBack = BSS_BACK_DIS - backDis;
turnLM = MOVE_BACK_TP - errorFront * FSS_BACK_Kp / 1000 + errorBack * BSS_BACK_Kp / 1000;
turnRM = MOVE_BACK_TP + errorFront * FSS_BACK_Kp / 1000 - errorBack * BSS_BACK_Kp / 1000;
}
msleep(150);
while(digital(TSS) == 0 && frontDis > 295){
frontDis = analog_et(FSS);
errorFront = FSS_BACK_DIS - frontDis;
turnLM = MOVE_BACK_TP - errorFront * FSS_BACK_Kp / 1000;
turnRM = MOVE_BACK_TP + errorFront * FSS_BACK_Kp / 1000;
motor(LM, turnLM);
motor(RM, turnRM - 2);
msleep(50);
}
while(analog(RSS) < RSS_OFFSET){
motor(RM, -82);
motor(LM, -90);
}
while(analog(RSS) > RSS_OFFSET){
motor(RM, -82);
motor(LM, -90);
}
while(analog(RSS) < RSS_OFFSET){
motor(RM, -82);
motor(LM, -90);
}
}
int main()
{
int i;
TurnCCW();//don't know what turn function we're using
for(i=0; i < 5; i++)
{
create_drive_straight(100);
sleep(2);
CenterCamera2();
}
for(create_drive_straight(-200); !digital(TouchSensor);){}
}
void bringback2cube(){
//claw up
motor(Motor_Up,Motor_up_speed);
//zurück 1s
drive(1000,-Drivespeed,-Drivespeed);
//turn right 90
drive(970,Turnspeed,-Turnspeed);
//vor to calibrate
drive(1000,Drivespeed,Drivespeed);
//back
drive(500,-Drivespeed,-Drivespeed);
//turn so little bit b4 cubes
drive(1130,Turnspeed,-Turnspeed);
//vor
drive(900,Drivespeed,Drivespeed);
//wait for claw up
claw_up();
//camera fix
camera_update();
//hide your cubes
cube_is_near();
//back
drive(1000,-Drivespeed,-Drivespeed);
//turn left more than 90 idk
drive(1330,-Turnspeed,Turnspeed);
//start to down motor
motor(Motor_Up,Motor_down_speed);
//vor to calibrate
drive(1000,Drivespeed,Drivespeed);
//langsamer weil wackeldackel
drive(2000,Drivespeed_middle+20,Drivespeed_middle+20);
//back
drive(400,-Drivespeed,-Drivespeed);
//turn more than 90 lulz
drive(980,-Turnspeed,Turnspeed);
//light left and shit
drive(5000,Drivespeed_middle*2,(Drivespeed_middle*2)-7);
//light back and shit
drive(1000,-Drivespeed_middle*2,(-Drivespeed_middle*2)+7);
//wait for claw down
while(!digital(Sensor_Down)){
if(seconds()>start+113){
claw_open();
disable_servos();
}
}
freeze(Motor_Up);
msleep(2000);
claw_open();
disable_servos();
}
void orientate()
{
//sensor left=0 right=1
//wheels left=1 right=0
int bool1=0;
int bool2=0;
set_motor_power(0, 100);
set_motor_power(1,100);
delay_ms(100);
while(1){
if(digital(1)==1)
{
set_motor_power(0,0);
delay_ms(100);
print_string("left det");
bool1++;
}
if(digital(0)==1)
{
set_motor_power(1,0);
delay_ms(100);
print_string("right det");
bool2++;
}
if(bool1>=1 && bool2>=1)
break;
}
set_motor_power(0,0);
set_motor_power(1,0);
delay_ms(100);
clear_screen();
print_string("done");
}
void cbc_touch(long speed, long timeout)
{
int i = 0;
mav(robot.leftWheel, speed);
mav(robot.rightWheel, speed);
while(!(digital(touch_left) && digital(touch_right)) && i < (timeout / 50))
{
if(digital(touch_left))
{
mav(robot.leftWheel, speed/10);
}
if(digital(touch_right))
{
mav(robot.rightWheel, speed/10);
}
i++;
msleep(50L);
}
off(robot.leftWheel);
off(robot.rightWheel);
}
void biofuelblock() {
turnCCW();
while(digital(12) == 0) {
movecenterfast(900);
}
movecenterbackwards(800);
sleep(032342);
ao();
turnCW();
set_servo_position(1,1900);
while(analog(4) <= 190 && analog(6) <= 190) {
movecenterslow(500);
}
ao();
}
void ClearWall()
{
if(get_create_lbump(.1) == 1 || get_create_rbump(.1) == 1)
{
create_drive_straight(-132); //one cube length (5 inches) + 5 mm safety
sleep(1);
create_stop();
}
if(digital(TouchSensor) == 1)
{
create_drive_straight(132);
sleep(1);
create_stop();
}
}
int main()
{
printf("Hello World\n");
while(digital(1)==0){
if (analog(1)>1000){
motor(0,100);
motor(2,80);
}
else {
motor(0,80);
motor(2,100);
}
}
ao();
return 0;
}
void bringback(){
//zurück zur linie
drive_till_line_backward();
//vor damit nicht insert into create
drive(1200,Drivespeed_middle,Drivespeed_middle);
//turn left 90
//drive(1100,-1*Drivespeed_middle-20,Drivespeed_middle+20);
motor(Motor_Left,-Drivespeed_middle*2);
while(analog(Sensor_Line_Left) < Sensor_Black){}
msleep(400);
while(analog(Sensor_Line_Left) > Sensor_Black){}
msleep(100);
freeze(Motor_Left);
//start claw down
motor(Motor_Up,Motor_down_speed);
//gerade but leicht links so it twerks
drive(9000,Drivespeed,Drivespeed);
wisch();
//vor to calibrate
drive(5000,Drivespeed_middle+20,Drivespeed_middle+20);
//back
drive(400,-Drivespeed,-Drivespeed);
//turn more than left 90
drive(980,-Turnspeed,Turnspeed);
//leicht rechts to calibrate
drive(5000,Drivespeed_middle*2,(Drivespeed_middle*2)-7);
//leicht back rechts to calibrate && würfel fit in tube
drive(800,-Drivespeed_middle*2,(-Drivespeed_middle*2)+7);
//wait for claw down
while(!digital(Sensor_Down)){}
freeze(Motor_Up);
msleep(2000);
claw_open();
msleep(2000);
}
int analog(int port)
{
if (port < 7)
{
return _raw_analog(port);
}
else if (port < 16)
{
return 255 * !digital(port);
}
else
{
printf("Analog port out of range\n");
beep();
return -1;
}
}
int main()
{
printf("Hello Create\n");
printf("press R button to start\n");
while(digital(13) ==0) {
msleep(20);
}
printf("Connecting\n");
create_connect(); // this engages the create
while(get_create_lbump()==0 && get_create_rbump()==0) {
create_drive_direct(100,-100);
}
create_stop();
create_disconnect();
msleep(500);
return 0;
}
// a LBA of sector requested
// p pointer to sector buffer
// returns TRUE if successful
int readSECTOR(LBA a, char *p)
{
int r, i;
#ifdef READ_LED
digitalwrite(READ_LED, 0);
#endif
// 1. send READ command
r = sendSDCmd(READ_SINGLE, (a << 9));
if (r == 0) // check if command was accepted
{
// 2. wait for a response
for(i=0; i<R_TIMEOUT; i++)
{
r = readSPI();
if (r == DATA_START)
break;
}
// 3. if it did not timeout, read 512 byte of data
if (i != R_TIMEOUT)
{
i = 512;
do{
*p++ = readSPI();
} while (--i>0);
// 4. ignore CRC
readSPI();
readSPI();
} // data arrived
} // command accepted
// 5. remember to disable the card
disableSD();
#ifdef READLED
digital(READ_LED, 1);
#endif
return (r == DATA_START); // return TRUE if successful
} // readSECTOR
void followLine()//making minor code for following the line
{
printf("1. Checking for touch.\n");
while (digital(10)==0)
{
printf("2. No touch. Checking reflect.\n");
while (analog(4)<=reflectValue)
{
printf("3. Reflect successful. Moving.\n");
motor(1,500);
motor(3,500);
msleep(500);
}
if (analog(4)>reflectValue)
{
printf("4. Reflect unsuccessful. Turning.\n");
motor(1,100);//turn around, make SMALL TURNS, outer pivot
countTime=0; //countTime resets here.
while (countTime<500 && analog(4)>=reflectValue) //countTime variable checks constantly and acts as a delay.
{ //if it either reaches 1000 or detects a line,
countTime=countTime+1; //it moves on and turns.
msleep(2);
}
if (countTime>=500)
{
printf("5. Count time exceeded 1000. Turning.\n");
while (analog(4)>=reflectValue)
{
motor(3,100);
msleep(1000);
ao();
}
}
else
{
printf("6. Reached end of if.\n");
}
} //no "else", because that could give a false positive more easily
printf("7.End of while loop. Deactivating motors...\n");
ao();
}
ao();
}
void follow_tape_left(int speed)
{
int i;
while(!digital(touch_left) && i < 50)
{
if(analog10(et_left)<850)
{
mav(robot.rightWheel,(int)((float)speed*1.15));
mav(robot.leftWheel,(int)((float)speed*0.85));
}
else
{
mav(robot.rightWheel,(int)((float)speed*0.85));
mav(robot.leftWheel,(int)((float)speed*1.15));
}
msleep(50L);
i++;
}
off(robot.rightWheel);
off(robot.leftWheel);
}
int avoid_cubeguy()
{
while (1)
{
mav(lego.left.port , -300);
mav(lego.right.port , -300);
msleep(10);
if (get_left() < THRESH)
break;
}
while (1)
{
t_line_follow();
if (digital(15) == 1)
break;
}
mav(lego.left.port , -300);
mav(lego.right.port , -300);
msleep(2500);
while (1)
{
mav(lego.left.port , 300);
mav(lego.right.port , -300);
if (get_middle() > THRESH && get_left() > THRESH && get_right() > THRESH)
break;
}
set_servo_position(arm_servo , ARM_OUT);
msleep(500);
while (1)
{
mav(lego.left.port , -300);
mav(lego.right.port , 300);
if (get_middle() < THRESH)
break;
}
return 0;
}
