/* Have 1/d forces which affect motion -3/d in the center, 1/d at each of 5 points, sampled randomly and replaced */ /* starts in middle of both */ #define CENTER_SCALE -3. #define SIDE_SCALE 1. #define LOWER_RANGE 10. #define UPPER_RANGE 10. #define LOWER_SCALE .5 #define UPPER_SCALE 1. #define LOWER_SLOT 3 #define UPPER_SLOT 1 #define LOWER_POT 0 #define UPPER_POT 2 #define RECCOUNT 5 #define COUNTLOOP 100 int recents[RECCOUNT][2]; int mlower = 0; int mupper = 0; int lower_eq; int upper_eq; void main() { int i; float lower_acc, upper_acc; int count = 0; int reccurr = 0; while (!start_button()); off(LOWER_SLOT); off(UPPER_SLOT); lower_eq = analog(LOWER_POT); upper_eq = analog(UPPER_POT); for (i = 0; i < RECCOUNT; i++) { recents[i][0] = (int) (LOWER_RANGE * sin(2. * 3.14159 * ((float) i) / ((float) RECCOUNT))); recents[i][1] = (int) (UPPER_RANGE * cos(2. * 3.14159 * ((float) i) / ((float) RECCOUNT))); } while (!stop_button()) { printf("%d: M: %d, %d; P: %d, %d\n", count, mlower, mupper, analog(LOWER_POT), analog(UPPER_POT)); lower_acc = CENTER_SCALE * LOWER_SCALE * ((float) (analog(LOWER_POT) - lower_eq)) / (safe(analog(LOWER_POT), lower_eq) / LOWER_RANGE); upper_acc = CENTER_SCALE * UPPER_SCALE * ((float) (analog(UPPER_POT) - upper_eq)) / (safe(analog(UPPER_POT), upper_eq) / UPPER_RANGE); for (i = 0; i < RECCOUNT; i++) { lower_acc += SIDE_SCALE * LOWER_SCALE * ((float) (analog(LOWER_POT) - recents[i][0])) / (safe(analog(LOWER_POT), recents[i][0]) / LOWER_RANGE); upper_acc += SIDE_SCALE * UPPER_SCALE * ((float) (analog(UPPER_POT) - recents[i][1])) / (safe(analog(UPPER_POT), recents[i][0]) / UPPER_RANGE); } mlower += (int) lower_acc; mupper += (int) upper_acc; if (mlower > 100) mlower = 100; if (mlower < -100) mlower = -100; if (mupper > 100) mupper = 100; if (mupper < -100) mupper = -100; if (mlower > 80 || mlower < -80) motor(LOWER_SLOT, mlower); else off(LOWER_SLOT); if (mupper > 80 || mupper < -80) motor(UPPER_SLOT, mupper); else off(UPPER_SLOT); count = (count + 1) % COUNTLOOP; recents[reccurr][0] = analog(LOWER_POT); recents[reccurr++][1] = analog(UPPER_POT); reccurr = reccurr % RECCOUNT; } } float safe(int a, int b) { if (a == b) return .5; else return (float) ((a - b)*(a - b)); }