Statistics: Posted by arekku — Wed Nov 25, 2009 4:54 pm

Code:

#include <ax12>
#include <BioloidController>

//Two variables to check if the servo serial line is receiving or transmiting
boolean Transmit = true; //True Tx is active, False Rx is active

int Direction = 0; // Digital pin for direction port

int FSR_Pin[5] = {0, 1, 2, 3};  // analog pins FSRs connect to
int FSR_Output[5];      // the analog reading from the FSR resistor divider
int FSROutputSum = 0;

int ForceMin = 600;  // minimum FSR reading to actuate motor
int MaxPos[5] = {700, 400, 700, 700};  // max position for each servo
int MinPos[5] = {650, 350, 650, 650};  // min position for each servo

int ServoID[5] = {1, 2, 3, 4};  // IDs of servos
int ServoPos[5] = {650, 350, 650, 650};  // current position of servos

void setup()
{
  digitalWrite(Direction, HIGH);
  pinMode(Direction,OUTPUT); //Set direction port as an output
 
  for(int i=0;i<=3;i++){
    SetSpeed(ServoID[i], 100);  // set all servos to speed of 100
  }

}

void loop(){
 
  while(Transmit){
   
    FSROutputSum = 0;
   
  for(int i=0;i<3> ForceMin){  // if an FSR is being pressed
   
     FSROutputSum = 0;
   
   for(int i=0;i<3> ForceMin){     // actuate appropriate motor
        if(ServoPos[i] == MaxPos[i]){
          ServoPos[i] = MinPos[i];
        }
        else{
          ServoPos[i] = MaxPos[i];
        }
        SetPosition(ServoID[i], ServoPos[i]);
        delay(250);
      }
      FSR_Output[i] = analogRead(FSR_Pin[i]);  // read FSRs again
      FSROutputSum += FSR_Output[i];
    }
   }
   
  }
 
}

Statistics: Posted by netphaze — Thu Nov 12, 2009 4:43 pm