//------------------------------------------------------------------------------
// CANAKIT h-bridge test #2
// 20111022 dan@marginallyclever.com
//------------------------------------------------------------------------------

#define IN1  (40)  // direction pins for motor 1
#define IN2  (41)
#define IN3  (42)  // direction pins for motor 2
#define IN4  (43)
#define IN5  (44)  // direction pins for motor 3
#define IN6  (45)
#define IN7  (46)  // direction pins for motor 4
#define IN8  (47)
#define IN9  (48)  // direction pins for motor 5
#define IN10 (49)
#define IN11 (50)  // direction pins for motor 6
#define IN12 (51)


// these values MUST remain sequential.
#define PWM1 ( 8)  // PWM speed control on motor 1
#define PWM2 ( 9)  // PWM speed control on motor 2
#define PWM3 (10)  // PWM speed control on motor 3
#define PWM4 (11)  // PWM speed control on motor 4
#define PWM5 (12)  // PWM speed control on motor 5
#define PWM6 (13)  // PWM speed control on motor 6

// Internally we treat speeds as 0...1
// analogWrite(PWM*) goes from 0 to 255.
#define SERVO_SCALE (255)


// these values MUST remain sequential.
#define SENSOR1 (0)  // the potentiometer for motor 1
#define SENSOR2 (1)  // the potentiometer for motor 2
#define SENSOR3 (2)  // the potentiometer for motor 3
#define SENSOR4 (3)  // the potentiometer for motor 4
#define SENSOR5 (4)  // the potentiometer for motor 5
#define SENSOR6 (5)  // the potentiometer for motor 6

//------------------------------------------------------------------------------
// pwm - PWM[N] motor defined above
//
// returns a value [0...1), 0 being fully retracted and 1 is fully extended.
//------------------------------------------------------------------------------
float ReadActuatorPos(int pwm) {
  int sensor;
  // sensor - SENSOR[N] defines above
  switch(pwm) {
  case PWM1:    sensor=SENSOR1;    break;
  case PWM2:    sensor=SENSOR2;    break;
  case PWM3:    sensor=SENSOR3;    break;
  case PWM4:    sensor=SENSOR4;    break;
  case PWM5:    sensor=SENSOR5;    break;
  case PWM6:    sensor=SENSOR6;    break;
  }

  return (float)analogRead(sensor) / 1024.0f;
}


//------------------------------------------------------------------------------
// pwm - see PWM[N] defines above
// newspeed - [0....1]
//------------------------------------------------------------------------------
void SetActuatorSpeed(int pwm,float newspeed) {
  int pin1, pin2, mode1, mode2;

  if( newspeed > 0 ) {
    mode1=HIGH;
    mode2=LOW;
    Serial.print("E");
  } else if( newspeed < 0 ) {
    mode1=LOW;
    mode2=HIGH;
    newspeed=-newspeed;
    Serial.print("R");
  } else {
    // mode1=HIGH, mode2=HIGH is the same as mode1=LOW, mode2=LOW
    mode1=HIGH;
    mode2=HIGH;
    Serial.print("L");
  }

  switch(pwm) {
  case PWM1:    pin1=IN1;    pin2=IN2;    Serial.print("pin1=IN1  pin2=IN2  ");  break;
  case PWM2:    pin1=IN3;    pin2=IN4;    Serial.print("pin1=IN3  pin2=IN4  ");  break;
  case PWM3:    pin1=IN5;    pin2=IN6;    Serial.print("pin1=IN5  pin2=IN6  ");  break;
  case PWM4:    pin1=IN7;    pin2=IN8;    Serial.print("pin1=IN7  pin2=IN8  ");  break;
  case PWM5:    pin1=IN9;    pin2=IN10;   Serial.print("pin1=IN9  pin2=IN10 ");  break;
  case PWM6:    pin1=IN11;   pin2=IN12;   Serial.print("pin1=IN11 pin2=IN12 ");  break;
  default: return;
  }

  Serial.print(pwm-PWM1+1);
  Serial.print("=");
//  Serial.print(newspeed);
//  Serial.print("=");
  Serial.print(mode1);
  Serial.print("/");
  Serial.print(mode2);
  Serial.print("\t");

  // set direction
  digitalWrite(pin1,mode1);
  digitalWrite(pin2,mode2);
  // set speed
  analogWrite(pwm,newspeed*SERVO_SCALE);
}


//------------------------------------------------------------------------------
void setup() {
  Serial.begin(57600);  // open serial link for debug output

  // prepare the pins that control the motor(s) movement modes
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);
  pinMode(IN5,OUTPUT);
  pinMode(IN6,OUTPUT);
  pinMode(IN7,OUTPUT);
  pinMode(IN8,OUTPUT);
  pinMode(IN9,OUTPUT);
  pinMode(IN10,OUTPUT);
  pinMode(IN11,OUTPUT);
  pinMode(IN12,OUTPUT);
}


//------------------------------------------------------------------------------
void loop() {
  // @TODO: Remove this resets all the acutators to their retracted position.
  // @TODO: it is temporary and should not be left in.
  int j,i;
  float f;
//*
  do {
    j=0;
    for(i=0;i<6;++i) {
      f=ReadActuatorPos(PWM1+i);
      if(f>0.51) SetActuatorSpeed(PWM1+i,-1);
      else if(f<0.49) SetActuatorSpeed(PWM1+i,1);
      else {
        SetActuatorSpeed(PWM1+i,0);
        ++j;
      }
    }
  } while(j<6);
/*/
  for(i=0;i<6;++i) {
    while(ReadActuatorPos(PWM1+i)>0.5) SetActuatorSpeed(PWM1+i,-1);
    while(ReadActuatorPos(PWM1+i)<0.5) SetActuatorSpeed(PWM1+i,1);
    SetActuatorSpeed(PWM1+i,0);
  }
//*/
}