void repeatAlways(void)
{
printf("%s%d\n","Thermometer reading is: ",analog(thermometer));
set_servo(leftServo, 15);
set_servo(rightServo, 75);
printf("%s%d\n","Temperature in Celsius is: ",(((analog(thermometer) - 32) * 5) /(float) 9));
}
void cross_turn_left()
{
unsigned int left,right;
while(1)
{
left = analog(0);
right = analog(2);
if(left>REF && right>REF)
{
forward(10);
}
else if(left<REF && right>REF)
{
turn_left(10);
}
else if(left>REF && right<REF)
{
turn_right(10);
}
else if(left<REF && right<REF)
{
forward(300);
turn_left(800);
break;
}
}
}
void main()
{
while(1)
{
lcd("L %d M %d R %d ",analog(0),analog(1),analog(2));
sleep(100);
}
}
void main()
{
while(1)
{
if ((analog(0)>AD0)&&(analog(1)>AD1))
for
int main()
{
}
void lego_drive_until_black_line(int direction, int speed, int line_size) {
int left_ticks_traveled = 0;
int right_ticks_traveled = 0;
float right_error;
float left_error;
float desired_value;
int left_threshold;
int right_threshold;
int right_sensor_value;
int left_sensor_value;
int left_multiplier;
int right_multiplier;
if (line_size == SMALL_LINE) {
left_threshold = LEFT_TOPHAT_SMALL_BLACK;
right_threshold = RIGHT_TOPHAT_SMALL_BLACK;
} else {
left_threshold = LEFT_TOPHAT_LARGE_BLACK;
right_threshold = RIGHT_TOPHAT_LARGE_BLACK;
}
clear_motor_position_counter(LEFT_MOTOR);
clear_motor_position_counter(RIGHT_MOTOR);
motor(LEFT_MOTOR, speed * direction);
motor(RIGHT_MOTOR, speed * direction);
while(TRUE) {
left_sensor_value = analog(LEFT_TOPHAT);
right_sensor_value = analog(RIGHT_TOPHAT);
if (left_sensor_value > left_threshold || right_sensor_value > right_threshold) {
break;
}
left_ticks_traveled = get_motor_position_counter(LEFT_MOTOR);
right_ticks_traveled = get_motor_position_counter(RIGHT_MOTOR);
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
left_error = desired_value - left_ticks_traveled;
right_error = desired_value - right_ticks_traveled;
left_multiplier = (int) ((left_error * l_kP) + 0.5);
right_multiplier = (int) ((right_error * r_kP) + 0.5);
motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (speed * direction) + right_multiplier);
}
freeze(LEFT_MOTOR);
freeze(RIGHT_MOTOR);
}
/*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);
}
}
}
u08 IR(u08 addr)
{
u16 dist = analog(0);
delay_ms(1);
dist += analog(0);
delay_ms(1);
dist += analog(0);
delay_ms(1);
dist += analog(0);
dist /= 4;
return dist;
}
u08 IR(u08 addr)
{
u16 dist = analog(addr);
delay_ms(1);
dist += analog(addr);
delay_ms(1);
dist += analog(addr);
delay_ms(1);
dist += analog(addr);
dist /= 4;
dist = (2112 / (dist + 28)) - 5;
return dist;
}
void adjust_onto_line(int direction) {
if (analog(L_TOPHAT) > 750 && analog(R_TOPHAT) > 750) {
return;
} else if (analog(L_TOPHAT) > 750 && analog(R_TOPHAT) < 750) {
motor(RIGHT_MOTOR, 30 * direction);
while (analog(R_TOPHAT) < 750) {}
off(RIGHT_MOTOR);
} else if (analog(L_TOPHAT) < 750 && analog(R_TOPHAT) > 750) {
motor(LEFT_MOTOR, 30 * direction);
while (analog(L_TOPHAT) < 750) {}
off(LEFT_MOTOR);
} else if (analog(L_TOPHAT) < 750 && analog(R_TOPHAT) < 750) {
printf("ROBOT IS NOT ON BLACK LINE!\n");
}
}
//Methode zum starten(warten auf Startlampe)
void starting(void){
servo_arc(0, 54);
if (analog(LEFT_BACK)>WHITE){ //wenn links schwarz
start = 'l';
}else if (analog(RIGHT_BACK)>WHITE){ //wenn rechts schwarz
start = 'r';
}
motPowLeft=8;
motPowRight=8;
lcd_putchar(start);
while(analog(9)>100);
timeoutAt = akt_time() + RUNTIME;
setMotPow(motPowLeft,motPowRight); //Gas geben:-)
sperrtimeout = akt_time() + 400; //Sperrzeit zum Linienzählen
}
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();
}
int main(void) {
initialize();
print_string("BotShop");
next_line();
print_string("Wall Following");
/*****************************************************************
* set timer 0 to trigger the interrupt 100 times per second.
* the OCR register sets how many counts between interrupts.
* the prescaler set how many clock cycles for every count.
* 16MHz / prescaler / OCR = interrupts per second.
/*****************************************************************/
TCCR0 |= _BV(CS02) | _BV(CS00); // set prescaler to 1024
OCR0 = 156;
while( !get_sw1() ); // wait for button to be pressed
while( get_sw1() ); // wait for button to be released
set_point = analog(RANGE_SENSOR);
// enable timer 0 interrupt
TIMSK |= _BV(OCIE0); // enable timer0 interrupt
// start moving
move( BASESPEED, BASESPEED );
// the while loop will spin forever so the program never ends
// the interrupt handler interrupts this loop to execute its code at regular intervals
while( 1 );
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);
}
void wait_for_light(int light_port_)
{
int l_on_, l_off_, l_mid_, t, good=0;
/* print statements may look funny but are spaced for LCD*/
while(!good){
printf ("Cal. with sensoron port #%d\n", light_port_);
sleep(2.);
printf("Cal: press Left when light on\n");
while(!left_button());
l_on_=analog(light_port_); /*sensor value when light is on*/
beep();
printf("Cal: light on value is=%d\n", l_on_);
sleep(1.);
beep();
printf("Cal: press Right when light off\n");
while(!right_button());
l_off_=analog(light_port_); /*sensor value when light is off*/
beep();
printf("Cal: light off value is=%d\n", l_off_);
sleep(1.);
beep();
if((l_off_-l_on_)>=15){ /*bright = small values */
l_mid_=(l_on_+l_off_)/2;
printf("Good CalibrationDiff=%d Waiting\n",(l_off_-l_on_));
good=1;
while(analog(light_port_)>l_mid_)
tone(440.,100);
}
else{
if(l_off_<128){
printf("Bad Calibration Add Shielding!!\n");
for(t=100; t<10000; t=t+200) tone((float)t,50);
}
else{
printf("Bad Calibration Aim sensor!!\n");
for(t=10000; t>200; t=t-200) tone((float)t,50);
}
}
}
}
void ForwardTouch ()
{
while (analog(TouchSensor) == 0)
{
create_drive_straight(-500);
}
create_stop();
}
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 goRightNoSuper() {
moveXAdjusted(30);
moveY(0);
while(analog(0) < 127) {
delayMs(50);
}
moveY(0);
moveX(0);
}
int avg_analog (int port){
int sum = 0;
for (int i=0; i< 10; i++)
sum += analog(port);
return sum / 10;
}
void anperr(
char *mess,
char *insert,
ANFSET *follow,
short line,
short col)
/* Calls the error logging routine.
*
* Effectuates the parser error recovery strategy - Panic mode.
* Skip input symbols until one is found which is a member in the
* follower set (or until EOF)
* follow = NULL --> empty set
*
*
* In: *mess => Error message code.
* *insert => Insert string.
* *follow => Follower set.
* line => Source line number.
* col => Source column counter.
*
* Out:
*
* (C)microform ab 1985-09-03 M. Nelson
*
* 1999-04-21 Rewritten, R. Svedin
*
******************************************************!*/
{
bool skip = FALSE; /* Symbols have been discarded? */
analog(line, col, mess, insert);
if (!aneset(follow)) /* Empty set? */
{
while (!aniset(sy.sytype, follow) && (sy.sytype != ANSYEOF))
{
anascan(&sy);
skip = TRUE;
}
}
if (skip)
analog((short)sy.sypos.srclin,(short)sy.sypos.srccol, "AN9991", "");
}
void main()
{
int infrared=0;
while(1)
{
infrared = analog(3);
lcd("Infrared: %d ",infrared);
sleep(100);
}
}
void main()
{
int light=0;
while(1)
{
light = analog(1);
lcd("Light: %d ",light);
sleep(100);
}
}
void moveFor(){
int frontDis = analog_et(FSS);
int backDis = analog_et(BSS);
int disDiff = frontDis - backDis;
int turnLM = MOVE_FOR_TP - disDiff * KDp;
int turnRM = MOVE_FOR_TP + disDiff * KDp;
while(frontDis > 295){
motor(LM, turnLM);
motor(RM, turnRM);
frontDis = analog_et(FSS);
backDis = analog_et(BSS);
printf("%d\t%d\n", frontDis, backDis);
disDiff = frontDis - backDis;
turnLM = MOVE_BACK_TP - disDiff * KDp;
turnRM = MOVE_BACK_TP + disDiff * KDp;
}
clear_motor_position_counter(LM);
clear_motor_position_counter(RM);
mtp(LM, 800, 500);
mtp(RM, 800, 500);
bmd(LM);
bmd(RM);
clear_motor_position_counter(LM);
clear_motor_position_counter(RM);
mtp(LM, 800, 260);
mtp(RM, -800, -260);
bmd(LM);
bmd(RM);
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);
}
}
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);
}
}
void drive_till_line_backward(){
motor(Motor_Left,-Drivespeed_middle);
motor(Motor_Right,-Drivespeed_middle);
printf("Start\n");
while(1){
if(analog(Sensor_Line_Left)>Sensor_Black&&analog(Sensor_Line_Right)>Sensor_Black){
printf("Abgehn\n");
break;
}
if(analog(Sensor_Line_Left)>Sensor_Black){
freeze(Motor_Right);
freeze(Motor_Left);
printf("Links\n");
motor(Motor_Right,-Drivespeed_middle);
msleep(50);
while(analog(Sensor_Line_Right)<Sensor_Black){}
break;
}
if(analog(Sensor_Line_Right)>Sensor_Black){
freeze(Motor_Right);
freeze(Motor_Left);
printf("Right\n");
motor(Motor_Left,-Drivespeed_middle);
msleep(50);
while(analog(Sensor_Line_Left)<Sensor_Black){}
break;
}
}
freeze(Motor_Left);
freeze(Motor_Right);
}
void drive_till_line(){
motor(Motor_Left,Drivespeed);
motor(Motor_Right,Drivespeed);
printf("Start\n");
while(1){
if(analog(Sensor_Line_Left)>Sensor_Black&&analog(Sensor_Line_Right)>Sensor_Black){
printf("Abgehn\n");
break;
}
if(analog(Sensor_Line_Left)>Sensor_Black) {
freeze(Motor_Left);
printf("Links\n");
motor(Motor_Right,Turnspeed);
msleep(50);
while(analog(Sensor_Line_Right)<Sensor_Black){}
break;
}
if(analog(Sensor_Line_Right)>Sensor_Black) {
freeze(Motor_Right);
printf("Right\n");
motor(Motor_Left,Turnspeed);
msleep(50);
while(analog(Sensor_Line_Left)<Sensor_Black){}
break;
}
}
freeze(Motor_Left);
freeze(Motor_Right);
//fahr back so claws doesnt break
msleep(500);
drive(500,-Drivespeed_middle,-Drivespeed_middle);
claw_up_delayed();
}
void lego_spin_until(int direction, int speed, int sensor, int (*comparator)(int, int), int threshold) {
int left_ticks_traveled = 0;
int right_ticks_traveled = 0;
float right_error;
float left_error;
float desired_value;
int left_multiplier;
int right_multiplier;
clear_motor_position_counter(LEFT_MOTOR);
clear_motor_position_counter(RIGHT_MOTOR);
motor(LEFT_MOTOR, speed * direction);
motor(RIGHT_MOTOR, -speed * direction);
while(TRUE) {
if (comparator(analog(sensor), threshold)) {
break;
}
display_printf(0, 0, "%4i", analog(sensor));
left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR));
right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR));
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
left_error = desired_value - left_ticks_traveled;
right_error = desired_value - right_ticks_traveled;
left_multiplier = (int) ((left_error * spin_l_kP) + 0.5);
right_multiplier = (int) ((right_error * spin_r_kP) + 0.5);
motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (-speed * direction) + right_multiplier);
}
freeze(LEFT_MOTOR);
freeze(RIGHT_MOTOR);
}
int digital(int port)
{
if (port < 7) /* analogs */
return analog(port) < 255;
if (port == 7) /* TIC1 */
return !(peek(0x1000) & 1);
if (port == 8) /* TIC2 */
return !(peek(0x1000) & 2);
if (port == 9) /* PAI */
return !(peek(0x1000) & 128);
if (port < 16) /* normal bit of 7fff as gotten from digital chip */
return !((peek(0x7fff) >> (port - 10)) & 1);
else {
int view_average_port(int port,int samples)
{
int i,accum;
printf("Press button to average\n");
sleep(.7);
while(chosen_button()==NEITHER_B)
{
printf("Port %d=%d\n",port,analog(port));
sleep(.1);
}
if(chosen_button()==ESCAPE_B)
return(-1);
for(i=0; i<samples; i++)
accum+=analog(port);
samples=(accum/samples);
printf("Port %d average=%d\n",port,samples);
return(samples);
}
int Lookdown()
{
int wAmb;
int i;
wAmb = analog(4);
DownLight(1);
wLD = wAmb - analog(4);
DownLight(0);
if (wLD <= wBlack + wSlop) iSight = iBlack;
else if (wLD >= wWhite - wSlop) iSight = iWhite;
else iSight = iMid;
for (i = 0; i < 3; i++)
{
rgwTable[i] = analog(16+i);
if (rgwTable[i] < rgwTableMid[i])
rgiTable[i] = iWhite;
else
rgiTable[i] = iBlack;
}
}
void main()
{
int infrared=0;
while(1)
{
infrared = analog(3);
lcd("Infrared: %d ",infrared);
if(infrared<500)
{
beep_d(6);
}
sleep(100);
}
}
