/* rb4_09.nqc */ /* Copyright (c) 1999 Robert Munafo. This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. */ /* Modification of rb_08. Cleanup of rb4_08, and minor bug fixes. Main change is to combine the is_2 and master tasks. things still left to do: - light level stats - verify gap in data collection between bumper=50 and bumper=30 - IR remote controls to adjust SCALE_NUMER - small demo programs that demonstrate input (also serves as a sensor test program) and motor control and perhaps: - modify chassis for rubber-band transfer to smooth out motion - decorate chassis to make front look more different from back - add a vertical rod to serve as a "training leash" */ // SCALE_NUMER and SCALE_DENOM determine how much the measured speeds // are scaled up (or down) to convert to output speeds. I found that // the motors move slower in GO mode than in TRAIN mode unless I scale // the average readings up by 2 or 3. #define SCALE_NUMER 3 #define SCALE_DENOM 2 #define MASTER_DELAY 5 #define TRAIN_DELAY 1 #define MC_DELAY 1 #define SD_DELAY 10 // light input int light; // 0 // motor inputs and motor speed control (four variables are shared // for these) int speed_a; // 1 int error_a; // 2 int speed_c; // 3 int error_c; // 4 #define raw_1 error_a #define raw_3 error_c // sum and difference of motor speeds. Used in both modes (input // and output) // decay values are useful for the min/max light readings. int rot_ac; int decay_r; int fwd_ac; int decay_f; // accumulated rot and fwd for standard (non-stimulus) motion int m0_f; int m0_r; int ft; #define n_0 ft // accumulated rot and fwd for bumper stimuli int m1_f; int m1_r; int rt; #define n_1 rt int m3_f; int m3_r; int td; #define n_3 td // feedback what type of data was taken int dv; // bumper inputs int bump_1; int bump_3; // mode control temp variables -- used only while in idle mode #define i_i speed_a #define i_mode speed_c #define minmax(t, v) if ((v) > 100) {t=100;} else if ((v) < -100) {t=-100;} else {t=v;} /* sd_1 computes raw motor speeds from the speed and rotation variables */ task sd_1 { while(true) { minmax(speed_a, fwd_ac + rot_ac) minmax(speed_c, fwd_ac - rot_ac) Sleep(SD_DELAY); } } // mc_1 does the floyd-steinberg motor control // GO mode only task mc_1 { while(true) { error_a = error_a + speed_a; if (error_a > 50) { Fwd(OUT_A, 7); error_a = error_a - 100; } else if (error_a < -50) { Rev(OUT_A, 7); error_a = error_a + 100; } else { Off(OUT_A); } error_c = error_c + speed_c; if (error_c > 50) { Fwd(OUT_C, 7); error_c = error_c - 100; } else if (error_c < -50) { Rev(OUT_C, 7); error_c = error_c + 100; } else { Off(OUT_C); } Sleep(MC_DELAY); } } // is_1 contains the input code that is always enabled. task is_1 { while (true) { light = IN_2; Sleep(SD_DELAY); } } // is_3 is used only in TRAIN mode. It reads the inputs and turns them // into useful sensor values task is_3 { while (true) { raw_1 = IN_1; if (raw_1 > 700) { // do motor speed speed_a = raw_1 - 924; minmax(speed_a, speed_a) // do bumper decay bump_1 = (bump_1 * 20) / 21; } else { speed_a = 0; bump_1 = 100; } raw_3 = IN_3; if (raw_3 > 700) { // do motor speed speed_c = raw_3 - 924; minmax(speed_c, speed_c) // do bumper decay bump_3 = (bump_3 * 20) / 21; } else { speed_c = 0; bump_3 = 100; } rot_ac = (speed_a - speed_c) / 2; fwd_ac = (speed_a + speed_c) / 2; decay_r = ((20*decay_r) + rot_ac) / 21; decay_f = ((20*decay_f) + fwd_ac) / 21; if ((bump_1 > 50) && (bump_1 > bump_3) && (n_1 < 327)) { m1_f += fwd_ac; m1_r += rot_ac; n_1 += 1; dv = 2; } else if ((bump_3 > 50) && (n_3 < 327)) { m3_f += fwd_ac; m3_r += rot_ac; n_3 += 1; dv = 3; } else if ((bump_1 > 30) || (bump_3 > 30)) { // nothing dv = 0; } else if (n_0 < 327) { m0_f += fwd_ac; m0_r += rot_ac; n_0 += 1; dv = 1; } else { dv = 0; } if (dv == 2) { PlayNote(1000, 1); } else if (dv == 3) { PlayNote(1500, 1); } else if (dv == 1) { PlayNote(55, 1); } Sleep(TRAIN_DELAY); } } task master { while(true) { if (IN_1 > 700) { // do bumper decay bump_1 = (bump_1 * 20) / 21; } else { bump_1 = 100; } if (IN_3 > 700) { // do bumper decay bump_3 = (bump_3 * 20) / 21; } else { bump_3 = 100; } // calculate weighted average sum from bumper stimuli ft = bump_1 * m1_f; rt = bump_1 * m1_r; td = bump_1; ft += (bump_3 * m3_f); rt += (bump_3 * m3_r); td += bump_3; // add default movement if any if (td < 100) { td = 100 - td; ft += (td * m0_f); rt += (td * m0_r); td = 100; } ft /= td; minmax(fwd_ac, ft) rt /= td; minmax(rot_ac, rt) Sleep(MASTER_DELAY); } } #define IDLE 0 #define ONE_PULSE 1 #define ENTER_GO 2 #define ENTER_TRAIN 3 task main { // initialize the hardware SensorMode(IN_1, SMODE_RAW); SensorType(IN_2, STYPE_LIGHT); SensorMode(IN_3, SMODE_RAW); Off(OUT_A); Off(OUT_C); rot_ac = 0; decay_r = 0; fwd_ac = 0; decay_f = 0; m0_f = 50; m0_r = 0; m1_f = -50; m1_r = 25; m3_f = 50; m3_r = 25; start is_1; while(true) { // idle mode PlayNote(196, 20); stop sd_1; stop mc_1; stop is_3; i_mode = IDLE; // nothing to do until they turn the light sensor on. wait (light > 1) { Sleep(5); } PlayNote(262, 10); i_i = 20; while (i_i > 0) { Sleep(5); i_i -= 1; if (light < 2) { i_mode = ONE_PULSE; i_i = 0; // no need to wait any longer } } if (i_mode == IDLE) { // it's still on i_mode = ENTER_GO; } else { // we got a pulse, need to wait a while now to see // if they turn it on again PlayNote(330, 10); i_i = 20; while (i_i > 0) { Sleep(5); i_i -= 1; if (light > 1) { i_mode = ENTER_TRAIN; i_i = 0; // no need to wait any longer } } } if (i_mode == ENTER_GO) { // still on -- that means run PlayNote(392, 10); // take measured readings and send to output fwd_ac = 0; rot_ac = 0; speed_a = 0; speed_c = 0; error_a = 0; error_c = 0; bump_1 = 0; bump_3 = 0; start sd_1; start mc_1; start master; wait (light < 2) { Sleep(10); } stop master; stop sd_1; stop mc_1; Off(OUT_A); Off(OUT_C); } else if (i_mode == ENTER_TRAIN) { // input mode // zero out our statistics n_0 = 20; m0_f *= n_0; m0_r *= n_0; // try to maintain the status quo bump_1 = 0; bump_3 = 0; n_1 = 5; m1_f *= n_1; m1_r *= n_1; n_3 = 5; m3_f *= n_3; m3_r *= n_3; // motors need to coast now Float(OUT_A); Float(OUT_C); // collect data start is_3; wait (light < 2) { Sleep(10); } stop is_3; // process results. m0_f /= n_0; m0_r /= n_0; m0_f = (SCALE_NUMER * m0_f)/SCALE_DENOM; minmax(m0_f, m0_f) m0_r = (SCALE_NUMER * m0_r)/SCALE_DENOM; minmax(m0_r, m0_r) m1_f /= n_1; m1_r /= n_1; m1_f = (SCALE_NUMER * m1_f)/SCALE_DENOM; minmax(m1_f, m1_f) m1_r = (SCALE_NUMER * m1_r)/SCALE_DENOM; minmax(m1_r, m1_r) m3_f /= n_3; m3_r /= n_3; m3_f = (SCALE_NUMER * m3_f)/SCALE_DENOM; minmax(m3_f, m3_f) m3_r = (SCALE_NUMER * m3_r)/SCALE_DENOM; minmax(m3_r, m3_r) } } }