void start_r3d2(void)
{
if (!is_motor_powered())
{
start_motor();
}
enable_sensor();
}
void wind_right (void) {
#ifdef DEBUG
uart_puts ("wind_right\r\n");
#endif
set_wind_dir_right ();
bit_set (PORTD, FILL_WIND );
turns_done=0;
action=WIND_RIGHT;
delay(100);
start_motor();
}
void wind_left (void) {
#ifdef DEBUG
uart_puts ("wind_left\r\n");
#endif
set_wind_dir_left ();
bit_set (PORTD, FILL_WIND );
action=WIND_LEFT;
turns_done=0;
delay(100);
start_motor();
}
void fill (void) {
#ifdef DEBUG
uart_puts ("fill\r\n");
#endif
set_wind_dir_none ();
bit_clr (PORTD, FILL_WIND );
turns_done=0;
action=FILL;
delay(100);
start_motor();
}
void test_relais(void) {
bit_clr(PORTD, P_BIT4);
bit_clr(PORTD, P_BIT5);
bit_clr(PORTD, P_BIT6);
bit_clr(PORTD, P_BIT7);
bit_set(PORTD, WIND_CLOCK);
uart_puts ("0x80\r\n");
lcd_puts (0, "bit7 (wind-clock)\n");
delay(2000);
bit_set(PORTD, MOTOR);
uart_puts ("0x40\r\n");
lcd_puts (0, "bit5 (motor)\n");
delay(2000);
bit_set(PORTD, FILL_WIND);
uart_puts ("0x20\r\n");
lcd_puts (0, "bit6 (fill-wind)\n");
delay(2000);
bit_set(PORTD, WIND_ANTICLOCK);
uart_puts ("0x10\r\n");
lcd_puts (0, "bit4 (wind anticlock)\n");
delay(2000);
lcd_clrscr(0);
uart_puts ("mot\r\n");
lcd_puts (0, "motor\n");
start_motor();
delay(2000);
stop_motor();
delay(500);
uart_puts ("fill\r\n");
lcd_puts (0, "fill\n");
fill();
delay(2000);
stop_motor();
uart_puts ("windleft\r\n");
lcd_puts (0, "windleft\n");
delay(500);
set_wind_dir_left();
delay(5000);
stop_motor();
}
int flpy_read(struct fdd *d, uint32_t lba, uint8_t *buf, uint32_t nr_sectors)
{
int rc = 0;
uint8_t retries;
struct chs f_addr;
if (d->param->cmos_type == 0) return -1;
while (_busy) ; /* Wait while the floppy driver is already busy. BUSY WAIT! */
_busy = TRUE;
start_motor(d);
specify(d);
/* Only retry for 3 times */
for (retries = 0; retries < 3; retries++) {
/* Move head to right track */
if (flpy_seek(d, f_addr.c) == 0) {
/* If changeline is active, no disk is in drive */
if (inportb(d->fdc->base_port + FDC_DIR) & 0x80) {
DEBUG(DL_ERR, ("no disk in drive %d\n",
d->number));
rc = -1;
goto errorout;
}
rc = fdc_xfer(d, &f_addr, dma_addr, nr_sectors, FDC_READ);
if (rc == 0) break;
} else
rc = -1;
if (retries < 2) recalibrate(d);
}
/* Copy data from the DMA buffer into the caller's buffer */
if ((rc == 0) && buf)
;
errorout:
stop_motor(d);
_busy = FALSE;
return rc;
}
void motor_init(void)
{
// direction
sbi(DDRD, 4);
sbi(DDRD, 4);
// brake
sbi(DDRD, 5);
sbi(PORTD, 5);
motor_period = 0;
int0_init();
PWM_NG_TIMER_8BITS_INIT(2, TIMER_8_MODE_PWM,
TIMER1_PRESCALER_DIV_256);
PWM_NG_INIT8(&pwm, 2, 12, PWM_NG_MODE_NORMAL, NULL, 0);
start_motor();
}
int flpy_write(struct fdd *d, uint32_t lba, const uint8_t buf, uint32_t nr_sectors)
{
int rc = 0;
uint8_t retries;
struct chs f_addr;
if (d->param->cmos_type == 0) return -1;
while (_busy) ; // The BUSY WAIT!
_busy = TRUE;
start_motor(d);
specify(d);
for (retries = 0; retries < 3; retries++) {
/* Move head to right track */
if (flpy_seek(d, f_addr.c) == 0) {
/* If changeline is active, no disk is in drive */
if (inportb(d->fdc->base_port + FDC_DIR) & 0x80) {
DEBUG(DL_ERR, ("no disk in drive %d\n",
d->number));
rc = -1;
break;
}
rc = fdc_xfer(d, &f_addr, dma_addr, nr_sectors, FDC_WRITE);
if (rc == 0) break;
} else
rc = -1;
if (retries < 2 ) recalibrate(d);
}
stop_motor(d);
_busy = FALSE;
return rc;
}
task main ()
{
waitForStart(); // wait for start of tele-op phase
StartTask (update_gyro);
nMotorEncoder[leftmotor] = 0;
nMotorEncoder[rightmotor] = 0;
servo[Claw]=110;
start_motor (25, BACKWARD);
wait10Msec(200);
start_motor (25, FORWARD);
wait10Msec(60);
motor[leftmotor]=0;
motor[rightmotor]=0;
wait10Msec(100);
while (nMotorEncoder[slideL]<18000)
{
motor[slideL]=slide_speed;
motor[slideR]=slide_speed;
}
motor[slideL]=0;
motor[slideR]=0;
//start_motor (40, TURNRIGHT); //not reading gyro while running start_motor (0.4 seconds).
while (happy_angle>-90) //ramp up to 40, skip to 100 and run until 40 degrees. TURNED TOO FAR
{
nxtDisplayTextLine (3,"%f",happy_angle);
motor[leftmotor]=-70;
motor[rightmotor]=70;
}
motor[leftmotor]=0;
motor[rightmotor]=0;
wait10Msec(100);
servo[Claw]=50;
motor[leftmotor]=60;
motor[rightmotor]=60;
wait10Msec(80);
motor[leftmotor]=0;
motor[rightmotor]=0;
wait10Msec(100);
while (happy_angle>-95) //ramp up to 40, skip to 100 and run until 40 degrees.
{
nxtDisplayTextLine (3,"%f",happy_angle);
motor[leftmotor]=-50;
motor[rightmotor]=50;
}
motor[leftmotor]=0;
motor[rightmotor]=0;
motor[leftmotor]=100;
motor[rightmotor]=100;
wait10Msec(260);
motor[leftmotor]=0;
motor[rightmotor]=0;
wait10Msec(300);
while (nMotorEncoder[slideL]>0)
{
motor[slideL]=-slide_speed;
motor[slideR]=-slide_speed;
}
motor[slideL]=0;
motor[slideR]=0;
}
void r3d2_monitor(void *dummy)
{
uint16_t object_angle, object_duty_cycle;
double float_duty_cycle;
uint8_t irq_ctx;
uint16_t object_beginnig_detection_local, object_end_detection_local, motor_period_local;
// if new position is avaliable
if(position_updated_value == position_updated)// <= 3);
{
// recopy value on local variable to avoid modification during computation
IRQ_LOCK(irq_ctx);
object_beginnig_detection_local = object_beginnig_detection;
object_end_detection_local = object_end_detection;
motor_period_local = motor_period;
IRQ_UNLOCK(irq_ctx);
// robot position computation
object_duty_cycle = object_end_detection_local - object_beginnig_detection_local;
object_angle = object_beginnig_detection_local + object_duty_cycle/2;
detected_robot_angle = (((float)object_angle)*360)/motor_period_local;
detected_robot_angle = (detected_robot_angle + robot_detected_angle_offset);
if (detected_robot_angle > 360)
{
detected_robot_angle -= 360;
}
else if(detected_robot_angle <0)
{
detected_robot_angle += 360;
}
float_duty_cycle = (((float)object_duty_cycle)*360)/motor_period_local;
detected_robot_distance = surface_reflection_ratio/(2 * tan((float_duty_cycle/2)*M_PI/180));
// update flags
robot_detected_value = robot_detected;
position_updated_value = position_not_updated;
// reset robot detection timout
robot_detected_timout_value =0;
}
else
{
// robot decetion timout reached
if (robot_detected_timout_value >= robot_detected_timout_treshold)
{
robot_detected_value = robot_not_detected;
}
else
{
robot_detected_timout_value ++;
}
}
// value not protected from irq
if(motor_rotating_timout_value == 0 && is_motor_powered())
{
WARNING(0,"motor stopped, restarting");
start_motor();
motor_rotating_timout_value = motor_rotating_timout_treshold;
motor_error_value = motor_error;
}
else
{
motor_error_value = motor_error; /// XXX modify this flag that will toogle in case of motor error detection
motor_rotating_timout_value --;
}
}
