Exemple #1
0
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);
	}
}
Exemple #4
0
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);
}
Exemple #6
0
/*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;
}
Exemple #8
0
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");
	}
}
Exemple #10
0
//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);
}
Exemple #14
0
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();
}
Exemple #17
0
void goRightNoSuper() {
	moveXAdjusted(30);
	moveY(0);
	while(analog(0) < 127) {
		delayMs(50);
	}
	moveY(0);
	moveX(0);
}
Exemple #18
0
int avg_analog (int port){
  int sum = 0;

  for (int i=0; i< 10; i++)
      sum += analog(port);

  return sum / 10;

}
Exemple #19
0
        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);
	}
}
Exemple #21
0
void main()
{
	int light=0;
	while(1)
	{
		light = analog(1);
		lcd("Light: %d ",light);
		sleep(100);
	}
}
Exemple #22
0
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);
}
Exemple #27
0
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 {
Exemple #28
0
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);
}
Exemple #29
0
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);
	}
}