Exemple #1
0
void Acquired_Taste(){//dump
	motor_off();
	shake();
	ao();
	enable_servos();
	servo_set(0, 450,4);//half way up
	motor(MOT_LEFT, -40);
	motor(MOT_RIGHT, -30);//back up
	msleep(2000); 
	servo_set (0, 20, 2);//full up
	ao();
	motor(MOT_LEFT, 40);
	motor(MOT_RIGHT, 30);//unjam if jammed
	msleep(1000);
	motor(MOT_LEFT, -40);
	motor(MOT_RIGHT, -30);//redump
	msleep(1000);
	ao();
	disable_servos();
	msleep(2000);
	enable_servos();
	motor(MOT_LEFT, 60);
	motor(MOT_RIGHT, 30);
	servo_set (0, 700, 2);//forward and drop
	motor(MOT_LEFT, -40);
	motor(MOT_RIGHT, -30);
	servo_set (0, 800, 2);
	disable_servos();	
}
Exemple #2
0
static void blinker(void) {
	led_inv(RIGHT);
	led_inv(LEFT);

#ifdef MATRIX
	for(uint8_t x = 0; x < 3; ++x) {
		for(uint8_t y = 0; y < 3; ++y) {
			matrix_set(x, y, ((x + y) & 1) ^ inv);
		}
	}

	inv = !inv;
#endif

	wait_ms(500);

	if(button_clicked(RIGHT)) {
		motor_on();
	}

	if(button_clicked(LEFT)) {
		motor_off();
	}

}
Exemple #3
0
/**@brief Button event handler.
 *
 * @param[in]   pin_no   The pin number of the button pressed.
 */
static void button_event_handler(uint8_t pin_no)
{
    switch (pin_no)
    {
    case EVAL_BOARD_BUTTON_0:
        mlog_str("button 0 pressed\r\n");

        if (connected)
            ble_oto_send_step_count(&m_oto, get_step_count());

        motor_off();
        led_stop();
        break;

    case EVAL_BOARD_BUTTON_1:
        mlog_str("button 1 pressed\r\n");

        if (!connected)
            advertising_start();
        break;

    default:
        APP_ERROR_HANDLER(pin_no);
    }
}
Exemple #4
0
void motor_move(int32 motor, int32 pasos, int32 direccion){
    (motor == 1) ? output_high(PIN_A2):output_low(PIN_A2);
    (motor == 2) ? output_high(PIN_A3):output_low(PIN_A3);
    (motor == 3) ? output_high(PIN_A4):output_low(PIN_A4);
    printf("Motor: %Ld, pasos%Ld, dir %Ld\n\r",motor,pasos,direccion);
    motores2(pasos,direccion);
    motor_off();
}
Exemple #5
0
void backToMe() {
	ao();
	msleep(5000);
	cmpc(MOT_LEFT);
	motor(MOT_LEFT, -60);
	motor(MOT_RIGHT, -100);
	while(1){
		int val = analog10(lightB);
		if(val > BLACK_SEN_THRESH ){//on black
			motor(MOT_RIGHT, -1);
		}else{
			motor(MOT_RIGHT, -100);
		}
		msleep(100);
	}
	motor_off();
	msleep(5000);
}
Exemple #6
0
void low_power(void)
{
	/* Use as little power as possible */
	random_walk_disable();

	motor_off();
	fled_off();
			
	/* IR Led off */
	P4OUT &= ~1;
	P4DIR |= 1;
	P4SEL &= ~1;
	
	dint();

	_BIS_SR(LPM3_bits);
	while(1);
}
Exemple #7
0
void Tougue_Lash(){
	cmpc(MOT_LEFT);
	motor(MOT_LEFT, -50);
	motor(MOT_RIGHT, -100);
	while(1){
		int val = analog10(lightB);
		if(val > BLACK_SEN_THRESH ){//on black
			motor(MOT_RIGHT, -1);
		}else{
			motor(MOT_RIGHT, -100);
		}
		msleep(100);
		if(gmpc(MOT_LEFT) < -3500){
			break;
		}
	}
	motor_off();// extra bit
	//msleep(5000);
}
Exemple #8
0
void Devour(){//second section
	cmpc(MOT_LEFT);
	motor(REVOLVE_MOTOR, 100);
	motor(MOT_LEFT, 80);
	motor(MOT_RIGHT, 1);
	while(1){
		int val = analog10(lightR);
		if(val > BLACK_SEN_THRESH ){//on black
			motor(MOT_RIGHT, 1);
		}else{
			motor(MOT_RIGHT, 100);
		}
		msleep(100);
		if(gmpc(MOT_LEFT) > 4050){
			break;
		}
	}
	motor_off();
	motor(MOT_LEFT, 70);
	motor(MOT_RIGHT, 70);
	msleep(5000);//sauare up
}
Exemple #9
0
void callback_set_command_mode(const globe_epas::globe_epas_cmd::ConstPtr& msg) {
  command_mode = msg->command_mode;
  switch(msg->command_mode) {
  case globe_epas::globe_epas_cmd::OFF:  
    motor_off();
    break;
  case globe_epas::globe_epas_cmd::CURRENT_CONTROL:  
    break;
  case globe_epas::globe_epas_cmd::TORQUE_ASSIST:  
    enable_torque_assist();
    break;
  case globe_epas::globe_epas_cmd::SPEED_CONTROL:  
    break;
  case globe_epas::globe_epas_cmd::POSITION_CONTROL:  
    break;
  case globe_epas::globe_epas_cmd::POSITION_CONTROL_WITH_SPEED_LIMIT:
    break;
  case globe_epas::globe_epas_cmd::SET_INC_ENCODER_VALUE:
    break;
  default:
    ROS_INFO("Invalid Globe EPAS command mode specified: %d", msg->command_mode);
  }        
}
Exemple #10
0
void Chomp() {//first section
	cmpc(MOT_LEFT);
	motor(REVOLVE_MOTOR, 100);
	motor(MOT_LEFT, 80);
	motor(MOT_RIGHT, 1);
	while(1){
		if(gmpc(MOT_LEFT) > 6800){
			motor_off();
			msleep(500);
			break;
		}else if(gmpc(MOT_LEFT) > 4500 && gmpc(MOT_LEFT) < 5200){
			motor(MOT_RIGHT,90);
		}else{
			int val = analog10(lightR);
			if(val > BLACK_SEN_THRESH ){//on black
				motor(MOT_RIGHT, 1);
			}else{
				motor(MOT_RIGHT, 100);
			}
			msleep(100);
			printf("%d,%d\n",gmpc(MOT_LEFT),val);
		}
	}
}//7295
//Read the string and execute instructions
void process_string(uint8_t  *instruction) {
  uint8_t code;
  uint16_t k;
  float temp;
  //command commands = NULL;
  FloatPoint fp;


  //the character / means delete block... used for comments and stuff.
  if (instruction[0] == '/') 	{
    Serial.println("ok");
    return;
  }

  enable_steppers();
  purge_commands(); //clear old commands
  parse_commands(instruction); //create linked list of arguments
  if (command_exists('G')) {
    code = getValue('G');

    switch(code) {
    case 0: //Rapid Motion
      setXYZ(&fp);
      set_target(&fp);
      r_move(0); //fast motion in all axis
      break;
    case 1: //Coordinated Motion
      setXYZ(&fp);
      set_target(&fp);
      if (command_exists('F')) _feedrate = getValue('F'); //feedrate persists till changed.
      r_move( _feedrate );
      break;
    case 2://Clockwise arc
    case 3://Counterclockwise arc
      FloatPoint cent;
      float angleA, angleB, angle, radius, length, aX, aY, bX, bY;

      //Set fp Values
      setXYZ(&fp);
      // Centre coordinates are always relative
      cent.x = xaxis->current_units + getValue('I');
      cent.y = yaxis->current_units + getValue('J');

      aX = (xaxis->current_units - cent.x);
      aY = (yaxis->current_units - cent.y);
      bX = (fp.x - cent.x);
      bY = (fp.y - cent.y);

      if (code == 2) { // Clockwise
        angleA = atan2(bY, bX);
        angleB = atan2(aY, aX);
      } 
      else { // Counterclockwise
        angleA = atan2(aY, aX);
        angleB = atan2(bY, bX);
      }

      // Make sure angleB is always greater than angleA
      // and if not add 2PI so that it is (this also takes
      // care of the special case of angleA == angleB,
      // ie we want a complete circle)
      if (angleB <= angleA) angleB += 2 * M_PI;
      angle = angleB - angleA;

      radius = sqrt(aX * aX + aY * aY);
      length = radius * angle;
      int steps, s, step;
      steps = (int) ceil(length / curve_section);

      FloatPoint newPoint;
      for (s = 1; s <= steps; s++) {
        step = (code == 3) ? s : steps - s; // Work backwards for CW
        newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));
        newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));
        newPoint.z = zaxis->current_units;
        set_target(&newPoint);

        // Need to calculate rate for each section of curve
        feedrate_micros = (feedrate > 0) ? feedrate : getMaxFeedrate();

        // Make step
        r_move(feedrate_micros);
      }

      break;
    case 4: //Dwell
      //delay((int)getValue('P'));
      break;
    case 20: //Inches for Units
      _units[0] = X_STEPS_PER_INCH;
      _units[1] = Y_STEPS_PER_INCH;
      _units[2] = Z_STEPS_PER_INCH;
      curve_section = CURVE_SECTION_INCHES;
      calculate_deltas();
      break;
    case 21: //mm for Units
      _units[0] = X_STEPS_PER_MM;
      _units[1] = Y_STEPS_PER_MM;
      _units[2] = Z_STEPS_PER_MM; 
      curve_section = CURVE_SECTION_MM;
      calculate_deltas();
      break;
    case 28: //go home.
      set_target(&zeros);
      r_move(getMaxFeedrate());
      break;
    case 30://go home via an intermediate point.
      //Set Target
      setXYZ(&fp);
      set_target(&fp);
      //go there.
      r_move(getMaxFeedrate());
      //go home.
      set_target(&zeros);
      r_move(getMaxFeedrate());
      break;
    case 81: // drilling operation
      temp = zaxis->current_units;
      //Move only in the XY direction
      setXYZ(&fp);
      set_target(&fp);
      zaxis->target_units = temp;
      calculate_deltas();
      r_move(getMaxFeedrate());
      //Drill DOWN
      zaxis->target_units = getValue('Z') + ((abs_mode) ? 0 : zaxis->current_units);
      calculate_deltas();
      r_move(getMaxFeedrate());
      //Drill UP
      zaxis->target_units = temp;
      calculate_deltas();
      r_move(getMaxFeedrate());
    case 90://Absolute Positioning
      abs_mode = true;
      break;
    case 91://Incremental Positioning
      abs_mode = false;
      break;
    case 92://Set as home
      set_position(&zeros);
      break;
    case 93://Inverse Time Feed Mode
      break;  //TODO: add this 
    case 94://Feed per Minute Mode
      break;  //TODO: add this
    default:
      Serial.print("huh? G");
      Serial.println(code,DEC);
    }
  }
  if (command_exists('M')) {
    code = getValue('M');
    switch(code) {
    case 3: // turn on motor
    case 4:
      motor_on();
      break;
    case 5: // turn off motor
      motor_off();
      break;
    case 82:
      DDRC |= _BV(1);
      PORTC &= ~_BV(1);
      DDRC &= ~_BV(0);
      PORTC |= _BV(0);
      // setup initial position
      for (int i=0; i<20; i++) {
        k=0;
        PORTB |= _BV(5); //go down
        while(PINC & _BV(0)) {
          PORTB |= _BV(2);
          delayMicroseconds(1);
          PORTB &= ~_BV(2);
          delayMicroseconds(200);
          k++;
        }
        //print result for this point
        Serial.println(k,DEC);
        PORTB &= ~_BV(5);  //move up to origin        
        while (k--) {
          PORTB |= _BV(2);
          delayMicroseconds(1);
          PORTB &= ~_BV(2);
          delayMicroseconds(12.5*stepping);
        }
      }
      break;
    case 81:
      DDRC |= _BV(1);
      PORTC &= ~_BV(1);
      DDRC &= ~_BV(0);
      PORTC |= _BV(0);
      while(1) {
        if (PINC & _BV(0)) Serial.println("high");
        else Serial.println("low");
      }
      break;
    case 80: //plot out surface of milling area
      DDRC |= _BV(1);
      PORTC &= ~_BV(1);
      DDRC &= ~_BV(0);
      PORTC |= _BV(0);
      // setup initial position
      fp.x = 0;
      fp.y = 0;
      fp.z = 0;
      set_target(&fp);
      set_position(&fp);
      r_move(0);
      for (int i=0; i<160; i+=2) {
        for (float j=0; j<75; j+=2) {
          fp.x=i;
          fp.y=j;
          fp.z=0;
          set_target( &fp );
          r_move( 0 );
          k=0;
          PORTB &= ~(_BV(5)); //go down
          while(PINC & _BV(0)) {
            PORTB |= _BV(2);
            delayMicroseconds(1);
            PORTB &= ~_BV(2);
            delayMicroseconds(200);
            k++;
          }
          //print result for this point
          Serial.print(i,DEC);
          Serial.print(",");
          Serial.print(j,DEC);
          Serial.print(",");
          Serial.println(k,DEC);
          PORTB |= _BV(5);  //move up to origin        
          while (k--) {
            PORTB |= _BV(2);
            delayMicroseconds(1);
            PORTB &= ~_BV(2);
            delayMicroseconds(200);
          }
        }
      }
      break;
    case 90: //plot out surface of milling area
      DDRC |= _BV(1);
      PORTC &= ~_BV(1);
      DDRC &= ~_BV(0);
      PORTC |= _BV(0);
      // setup initial position
      fp.x = 0;
      fp.y = 135;
      fp.z = 0;
      set_target(&fp);
      set_position(&fp);
      r_move(0);
      for (int i=0; i<1; i++) {
        for (float j=135; j!=0; j-=.25) {
          fp.x=i;
          fp.y=j;
          fp.z=0;
          set_target( &fp );
          r_move( 0 );
          k=0;
          PORTB |= _BV(5); //go down
          while(PINC & _BV(0)) {
            PORTB |= _BV(2);
            delayMicroseconds(1);
            PORTB &= ~_BV(2);
            delayMicroseconds(200);
            k++;
          }
          //print result for this point
          Serial.print(i,DEC);
          Serial.print(",");
          Serial.print(j,DEC);
          Serial.print(",");
          Serial.println(k,DEC);
          PORTB &= ~_BV(5);  //move up to origin        
          while (k--) {
            PORTB |= _BV(2);
            delayMicroseconds(1);
            PORTB &= ~_BV(2);
            delayMicroseconds(200);
          }
        }
      }
      break;
    case 98: //M98 Find Z0 where it is one step from touching.
      DDRC |= _BV(1);
      PORTC &= ~_BV(1);
      DDRC &= ~_BV(0);
      PORTC |= _BV(0);
      PORTB |= _BV(5);
      while(PINC & _BV(0)) {
        PORTB |= _BV(2);
        delayMicroseconds(1);
        PORTB &= ~_BV(2);
        delayMicroseconds(200);
      }
      break;
    case 99: //M99 S{1,2,4,8,16} -- set stepping mode
      if (command_exists('S')) {
        code = getValue('S');
        if (code == 1 || code == 2 || code == 4 || code == 8 || code == 16) {
          stepping = code;
          setStep(stepping);
          break;
        }
      }
    default:
      Serial.print("huh? M");
      Serial.println(code,DEC);
    }
  }
  Serial.println("ok");//tell our host we're done.
}
Exemple #12
0
static void run(void) {
	if(ir_recv()) {
		// receive a ir signal, react

		motor_on();

		led_on(RIGHT);

		set_note(NOTE_B, 5);
		wait_ms(200);
		set_note(NOTE_F, 5);
		wait_ms(100);

		led_off(RIGHT);
		led_on(LEFT);

		set_note(NOTE_G, 5);
		wait_ms(100);
		set_note(NOTE_Ab, 5);
		wait_ms(100);
		set_note(NOTE_A, 5);
		wait_ms(100);

		led_off(LEFT);

		motor_off();
	} else if(button_clicked(RIGHT)) {
		// button clicked, send ir signal and do some stuff

		led_on(RIGHT);

		set_note(NOTE_A, 5);
		wait_ms(100);
		set_note(NOTE_Ab, 5);
		wait_ms(100);
		set_note(NOTE_G, 5);
		wait_ms(100);

		led_off(RIGHT);
		led_on(LEFT);

		set_note(NOTE_F, 5);
		wait_ms(100);
		set_note(NOTE_B, 5);
		wait_ms(200);
		stop_note();

		led_off(LEFT);

		ir_on();

		wait_ms(400);

		ir_off();

		wait_ms(10);
	} else {
		// regular bug behaviour

		uint8_t light = photons_measure();

		pentatonic_all_led_set(light >> 3);

		motor_set(biased_random(light) > BASELINE + 0x40);

		led_set(RIGHT, biased_random(light) > BASELINE + 0x00);
		led_set(LEFT, biased_random(light) > BASELINE + 0x00);

		if(biased_random(light) > BASELINE + 0x20) {
			uint16_t tone = (biased_random(light) * 2) + 500;
			set_note(tone, 0);
		} else {
			stop_note();
		}

		wait_ms(200);
	}
}
Exemple #13
0
// wildi ToDo this thread must go into a separate file
void *
oak_digin_thread(void * args)
{
  int ret ;
  // wildi ToDo int *values = xmalloc(devInfo.numberOfChannels * sizeof(int));
  int *values = malloc(200 * sizeof(int));
  int rd_stat;
  int bits = 0xff;
  int print_bits = 0xff;
  int bit;
  int i ;
  int lastDoorStateTest= DS_UNDEF ;
  int lastDoorState= DS_UNDEF ;
  int stop_motor= STOP_MOTOR_UNDEFINED ;
  char bits_str[32] ;
  struct timespec sl ;
  struct timespec rsl ;
  static struct timeval time_last_stat;
  static struct timeval time_enq;
  gettimeofday(&time_last_stat, NULL);
  // debug
  int print =0 ;
  oak_thread_state= THREAD_STATE_UNDEFINED ; // make it "reentrant"

  sl.tv_sec= 0. ;
  sl.tv_nsec= (long ) 200 * 1000 * 1000 ;

  if( test_oak== TEST_OAK) {
    fprintf( stderr,  "oak_digin_thread: test_oak== TEST_OK, sleep %d milliseconds\n", SLEEP_TEST_OAK_MILLISECONDS);
  } 
  fprintf( stderr,  "oak_digin_thread: thread started\n");

  while (1) {

    oak_digin_thread_heart_beat++ ;

    if( oak_digin_thread_heart_beat > HEART_BEAT_UPPER_LIMIT) {
      oak_digin_thread_heart_beat= 0 ;
    }
    rd_stat = readInterruptReport(oakDiginHandle, values);
    if ((rd_stat < 0) && (errno == EINTR)) {
      oak_thread_state= THREAD_STATE_UNDEFINED ;
      // wildi ToDo something sensible here values[1]=
      fprintf( stderr, "oak_digin_thread:  problems reading from oak device\n") ;
    }
    bits      = values[1] & 0xff ;
    print_bits= values[1] & 0xff ;
   
    print= 0 ;
    for (i = 0; i < 8; i++) {
      bit = (print_bits & 0x1) ;
      if( bit) {
	bits_str[7-i]= '1' ;
	print++ ;
      } else {
	bits_str[7-i]= '0' ;
      }
      print_bits = print_bits >> 1;
    }
    bits_str[8]= '\0' ;
    //fprintf( stderr, "oak_digin_thread: doorState=%d, bits %s\n",  doorState, bits_str) ;
    // turn off the motor first

    if((( bits & OAK_MASK_CLOSED) > 0) && (( doorState== DS_STOPPED_CLOSED) || ( doorState== DS_RUNNING_OPEN)))  {
      // let the motor open the door
      stop_motor= STOP_MOTOR_UNDEFINED ;

    } else if((( bits & OAK_MASK_OPENED) > 0) && (( doorState== DS_STOPPED_OPENED) || ( doorState== DS_RUNNING_CLOSE) || ( doorState== DS_RUNNING_CLOSE_UNDEFINED)))  {
      // let the motor close the door
      stop_motor= STOP_MOTOR_UNDEFINED ;

    } else if((( bits & OAK_MASK_CLOSED) > 0) && ( motorState== SSD650V_MS_RUNNING)) { // 2nd expr. necessary not to overload SSD650v on RS 232
      stop_motor= STOP_MOTOR ;
      fprintf( stderr, "oak_digin_thread: found ( bits & OAK_MASK_CLOSED) > 0), bits %s\n",  bits_str) ;

    } else if((( bits & OAK_MASK_OPENED) > 0) && ( motorState== SSD650V_MS_RUNNING)){
      stop_motor= STOP_MOTOR ;
      fprintf( stderr, "oak_digin_thread: found (bits & OAK_MASK_OPENED) > 0), bits %s\n",  bits_str) ;

    } else if ( (bits & OAK_MASK_END_SWITCHES) > 0) {  
      // the motor is turned off and eventually OAK devices too (no cummunication possible)
      stop_motor= STOP_MOTOR ;
    } else if ( (bits & OAK_MASK_END_SWITCHES) == 0) { // both end switches are zero 
      // the door is inbetween, (or beyond the software end switches :-(( = very close to the end switches)
      //fprintf( stderr, "oak_digin_thread:  might be a bad reading from oak device\n") ; // see above ToDo
      // wildi ToDo: not yet defined what to do
    } 

    // test
    if( test_oak== TEST_OAK) {
      if( lastDoorStateTest != doorState) {
	lastDoorStateTest= doorState ;
	gettimeofday(&time_last_stat, NULL);
	fprintf( stderr, "oak_digin_thread:  status change resetting timer\n") ;
      }
      if( motorState== SSD650V_MS_RUNNING) {
	gettimeofday(&time_enq, NULL);
	if (difftimeval(&time_enq, &time_last_stat) > (SLEEP_TEST_OAK_MILLISECONDS * 1000)) { // milliseconds
	  stop_motor= STOP_MOTOR ;
	  fprintf( stderr, "oak_digin_thread: motorState== (DS_RUNNING_OPEN|DS_RUNNING_CLOSE), stopping now, diff %d[msec]\n", (int)difftimeval(&time_enq, &time_last_stat)) ;
	  gettimeofday(&time_last_stat, NULL);
	  fprintf( stderr, "oak_digin_thread: stopping motor  after time ellapsed\n") ;
	}
      }
      if(( print > 0) &&  ( stop_motor != STOP_MOTOR)) {
	stop_motor= STOP_MOTOR ;
	fprintf( stderr, "oak_digin_thread: print stop_motor= STOP_MOTOR \n") ;
      }
    }
    // end test
    // manual mode, in case an endswitch erroneously indicates contact
    if(ignoreEndswitch==NOT_IGNORE_END_SWITCH){
    if( stop_motor== STOP_MOTOR) {
      
      fprintf( stderr, "oak_digin_thread: stopping motor now\n") ;
      while(( ret= motor_off()) != SSD650V_MS_STOPPED) { // 
	fprintf( stderr, "oak_digin_thread: can not turn motor off\n") ;
	ret= nanosleep( &sl, &rsl) ;
	if((errno== EFAULT) || ( errno== EINTR)||( errno== EINVAL ))  {
	  fprintf( stderr, "Error in nanosleep\n") ;
	  errno= 0 ;
	}
      }
      set_setpoint(0.);
      fprintf( stderr, "oak_digin_thread: motor stopped\n") ;
      stop_motor= STOP_MOTOR_UNDEFINED ;
      struct ln_date utm;
      ln_get_date_from_sys( &utm) ;
      sprintf( date_time, "%4d-%02d-%02dT%02d:%02d:%02d", utm.years, utm.months, utm.days, utm.hours, utm.minutes, (int) utm.seconds) ;

    }
    }
    // door status
    if( motorState== SSD650V_MS_STOPPED) {
      // change status here
      if( bits & OAK_MASK_CLOSED) { // true if OAK sees the door
	doorState= DS_STOPPED_CLOSED ;
	if( lastDoorState != doorState) {
	  lastDoorState= doorState ;
	  fprintf( stderr, "oak_digin_thread: state is DS_STOPPED_CLOSED\n") ;
	} 
      } else if( bits & OAK_MASK_OPENED) { // true if OAK sees the door
	doorState= DS_STOPPED_OPENED ;
	if( lastDoorState != doorState) {
	  lastDoorState= doorState ;
	  fprintf( stderr, "oak_digin_thread: state is DS_STOPPED_OPENED\n") ;
	}
      } else {
	doorState= DS_STOPPED_UNDEF ;
	lastDoorState= doorState ;
	//fprintf( stderr, "oak_digin_thread: state is DS_STOPPED_UNDEFINED\n") ;
      }
    } else if( motorState== SSD650V_MS_RUNNING) {
      // keep status here 
      if( doorState == DS_RUNNING_OPEN) {
      } else if( doorState == DS_RUNNING_CLOSE) {
      } else if( doorState == DS_RUNNING_CLOSE_UNDEFINED) {
      } else {
	// should occasianally occur if the door is stopped manually 
	doorState= DS_RUNNING_UNDEF ;
	lastDoorState= doorState ;
	//fprintf( stderr, "oak_digin_thread: motorState== SSD650V_MS_RUNNING, state is DS_RUNNING_UNDEF\n") ;
      }
    } else if( motorState== SSD650V_MS_UNDEFINED) {
      doorState= DS_UNDEF ;
      lastDoorState= doorState ;

      if( bits & OAK_MASK_OPENED) { // true if OAK sees the door
	fprintf( stderr, "oak_digin_thread: state is DS_UNDEF, door is opened, motor is undefined\n") ;
      } else if( bits & OAK_MASK_CLOSED) { // true if OAK sees the door
	fprintf( stderr, "oak_digin_thread: state is DS_UNDEF, door is closed, motor is undefined\n") ;
      } else {
	fprintf( stderr, "oak_digin_thread: state is DS_UNDEF, door is undefined, motor is undefined\n") ;
      }
    } else {
      // should never occur
      doorState= DS_UNDEF ;
      lastDoorState= doorState ;
      fprintf( stderr, "oak_digin_thread: state is DS_UNDEF, should never never occur\n") ;
    }

    CHECK_OAK_CALL(rd_stat); // wildi ToDo: what's that?
  } /* while (1) */
  
  return NULL;
}
Exemple #14
0
/*
 * And now, it's time to implenent the read or write function, that's
 * all the floppy driver mean!
 * 
 * Read/Write one sector once.
 */
static int floppy_rw(int sector, char *buf, int command)
{
	int head;
	char *dma_buffer = buf;
	static char tmp_dma_buffer[512];

	//LOG("TMP dma buffer: %p\n", tmp_dma_buffer);

        lba_to_chs(sector, &head, &track, &sector);
	LOG("head: %d \ttrack: %d \tsector: %d\n", head, track, sector);

        /* turn it on if not */
        motor_on();

        if (inb_p(FD_DIR) & 0x80) {
                changed = TRUE;
                seek(1, head);        /* clear "disk change" status */
                recalibrate();
                motor_off();
                printk("floppy_rw: Disk change detected. You are going to DIE:)\n");
                
                pause();  /* just put it in DIE */
        }

        /* move head to the right track */
        if (!seek(track, head)) {
                motor_off();
                printk("floppy_rw: Error seeking to track#%d\n", track);
                return FALSE;
        }
                
	if ((unsigned long)buf >= 0xff000) {
		dma_buffer = tmp_dma_buffer;
		if (command == FD_WRITE)
			memcpy(dma_buffer, buf, 512);
	}

        setup_DMA((unsigned long)dma_buffer, command);

        send_byte(command);
        send_byte(head<<2 | 0);
        send_byte(track);
        send_byte(head);
	send_byte(sector);
        send_byte(2);           /* sector size = 125 * 2^(2) */
        send_byte(floppy.sector);
        send_byte(0);
        send_byte(0xFF);        /* sector size(only two valid vaules, 0xff when n!=0*/

        if (!wait_fdc(FALSE)) {
                //LOG("wait fdc failed!\n");
                //return 0;
                /*
                printk("Time out, trying operation again after reset() \n");
                reset();
                return floppy_rw(sector, buf, command);
                */
        }

        motor_off();

        if (/*res != 7 || */(ST0 & 0xf8) || (ST1 & 0xbf) || (ST2 & 0x73) ) {
                if (ST1 & 0x02) 
                        LOG("Drive is write protected!\n");
                else
                        LOG("floppy_rw: bad interrupt!\n");

                return -EIO;
        } else {
		LOG("floppy_rw: OK\n");
		if ((unsigned long)buf >= 0xff000 && command == FD_READ)
			memcpy(buf, dma_buffer, 512);
		return 0;
        }
}