/* program for huge yellow lego 'servo motor' */

#define target 27
#define sensor 25
#define mmotor 0
#define powerswitch 7

void main()
{
  int s, t, m;

  while (digital(powerswitch) == 0) {
    sleep(0.2);
    printf("Feedback\n");
  }
  while (digital(powerswitch) == 1) {}
  while (digital(powerswitch) == 0) {
    sleep(0.05);
    s = analog(sensor);
    t = analog(target);
    printf("Target %d         Sensor %d\n",t,s);
    if (1 == 2) {
      /* stupid feedback */
      if (t > s) { motor(mmotor,100); }
      if (t < s) { motor(mmotor,-100); }
      if (t == s) { motor(mmotor,0); }
    }
    else {
      /* proportional feedback */
      m = t - s;
      if (m < 0) { m = m - 4; }
      if (m > 0) { m = m + 4; }
      if (m > 100) {m = 100;}
      if (m < -100) {m = -100;}
      motor(mmotor,m);
    }
  }
  printf("Done\n");
  off(mmotor);
}