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();
}
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();
}
}
/**@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);
}
}
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();
}
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);
}
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);
}
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);
}
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
}
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);
}
}
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.
}
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);
}
}
// 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;
}
/*
* 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, §or);
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;
}
}