Go Down

Topic: sweep servo not working ??? (Read 449 times) previous topic - next topic

Foureyes

Hi all
sweep servo not working ???

I have my sweep servo pluged in but it doesnt work it is pluged into termanal 6 and has seperate power supply the others work fine but not the sweep servo iam not using the srfo5 sensor

can any one explain why??

here is the code i stole):

int sensorIR = 0;
int sensorUS = 7;
int calibIR = 10000;
int valIR;
int valUS;

int sensorPot = 2;
int valPot;

int button = 5;
int valButton;
int turnOnOff=0;

int servoTU = 8;
int servoLF = 9;
int servoLB = 10;
int servoRF = 11;
int servoRB = 12;
unsigned long lastPulseR;
unsigned long lastPulseL;
unsigned long lastPulseTU;
int pulseWidthR = 1250;
int pulseWidthL = 1750;
int pulseWidthRTU = 0;
int pulseWidthLTU = 0;

int pulseWidthTU = 1710;
int centerTU = 1430;
int rangeTU=1360;

int refreshTimeServoL = 20;
int refreshTimeServoR = 20;
int refreshTimeServoTU = 20;

int led=3;

int angleDepart=0;
signed long angle=0;
int angleMin=-45;
int angleMax=45;
int nbAngle;
int distance[180];
int distanceMax[18];
int distanceMin[18];
int distanceMoy[18];
int direction[18];
int directionTU;
int sensTU=1;
int turn=0;
int n;

void servoL(int pulseWidth){
  if(millis()-lastPulseL >= refreshTimeServoL){
    digitalWrite(servoLF,HIGH);
    digitalWrite(servoLB,HIGH);
    delayMicroseconds(pulseWidth);
    digitalWrite(servoLF,LOW);
    digitalWrite(servoLB,LOW);
    lastPulseL=millis();
  }
}

void servoR(int pulseWidth){
  if(millis()-lastPulseR >= refreshTimeServoR){
    digitalWrite(servoRF,HIGH);
    digitalWrite(servoRB,HIGH);
    delayMicroseconds(pulseWidth);
    digitalWrite(servoRF,LOW);
    digitalWrite(servoRB,LOW);
    lastPulseR=millis();
  }
}

int servoT(int pulseWidth){
  if(millis()-lastPulseTU >= refreshTimeServoTU){
    digitalWrite(servoTU,HIGH);
    delayMicroseconds(pulseWidth);
    digitalWrite(servoTU,LOW);
    lastPulseTU=millis();
    return 1;
  }
  return 0;
}

int getPulseT(){
  int pulse;
  if(angle<=angleMin){
   sensTU=0;
  // displaySonar();
   //directionTU=getDirection();
  }
  if(angle>=angleMax){
   //directionTU=getDirection();
  // displaySonar();
   sensTU=1;
  }
  return (centerTU+((angle)*rangeTU/90));
}

int readSensorIR(){
  return (calibIR/analogRead(sensorIR));         
}

void setLed(int valLed){
  analogWrite(led,valLed);
}

int readSensorUS(){
  pinMode(sensorUS, OUTPUT);
  digitalWrite(sensorUS, LOW);
  delayMicroseconds(2);
  digitalWrite(sensorUS, HIGH);
  delayMicroseconds(10);
  digitalWrite(sensorUS, LOW);
  pinMode(sensorUS, INPUT);
  return(pulseIn(sensorUS, HIGH)/58);
}

int readSensorPot(){
  return (centerTU+long(analogRead(sensorPot))*1000/1024-500);
}                     

int readButton(){
  unsigned long contactTime;
  valButton=analogRead(button);
//Serial.print(valButton);
//Serial.print(" ");
  if(valButton<512)
   return turnOnOff;
  contactTime=millis();
  while(valButton>=512){
   valButton=analogRead(button);
   //Serial.print(valButton);
   //Serial.print("\n");
  }
if(millis()-contactTime<20)
   return turnOnOff;
angle=angleDepart;
return(1-turnOnOff);
}

int getDirection(){
int angle=0,i,j,n=0,seg=0,dir=4,maxSeg=0;
for(i=0;i<18;i++){
   distanceMax=0;
   distanceMin=10000;
   distanceMoy=0;
   for(j=0;j<5;j++){
      if(distance[n+j]>distanceMax)
       distanceMax=distance[n+j];
      if(distance[n+j]<distanceMin)
       distanceMin=distance[n+j];
     distanceMoy=distance[n+j];
     //Serial.print(distance[n+j]);
     //Serial.print(" ");
   }   
   direction=0;
   if(distanceMax>110&&distanceMin>55&&distanceMoy>77){
    seg++;
    if(seg>maxSeg)
    maxSeg=seg;
    direction=seg;
   }
   else
    seg=0;
   n+=5; 
}
for(i=0;i<18;i++){
   if(direction==maxSeg){
    dir=i-maxSeg/2;
    dir=dir*5-40;
   }
// Serial.print(direction);
}

//Serial.print(" dir ");
// Serial.print(" \n");
return dir; 
}

void displaySonar(){
  int n;
  for(n=0;n<nbAngle;n++){
   Serial.print(distance[n]);
   Serial.print(" ");
  }
Serial.print("\n");
}

void pilote2(int valUS,int valTU){
  int acc=0;
  directionTU=getDirection()/5;
  acc=(30*max(min(valUS,100),3)/100)-8;
  // Serial.print(acc,DEC);
  //Serial.print("acc   ");
  // Serial.print(directionTU);
  // Serial.print(" ");
   pulseWidthR = min(max(pulseWidthR-acc-directionTU,1250),1750);
   pulseWidthL = max(min(pulseWidthL+acc-directionTU,1750),1250);
   
  // Serial.print(pulseWidthR,DEC);
  // Serial.print("pR   ");
  // Serial.print(pulseWidthL,DEC);
  // Serial.print("pL   ");
  //Serial.print("\n");
}

void pilote(int valUS,int valTU){
  int acc=0,dirR=75,dirL=75;
  directionTU=getDirection();
  if(directionTU>0){
    dirR=75;
    dirL=75-150*directionTU/45;
   }
  if(directionTU<0){
    dirL=75;
    dirR=75-150*directionTU/45;
   }
 
  acc=(30*max(min(valUS,100),3)/100)-15;
  //Serial.print(acc,DEC);
  //Serial.print("acc   ");
  //Serial.print(directionTU);
  // Serial.print("directU   ");
 
  //Serial.print(dirL);
  //Serial.print(" dirL   ");
  // Serial.print(dirR);
  //Serial.print("dirR ");
 
 
  pulseWidthL = max(min(pulseWidthL+acc,1675),1325);
  pulseWidthR = min(max(pulseWidthR-acc,1325),1675);

  pulseWidthL = max(min(pulseWidthL+dirL,1750),1250); 
  pulseWidthR = min(max(pulseWidthR-dirR,1250),1750);

 
  // Serial.print(pulseWidthR,DEC);
  // Serial.print("pR   ");
  // Serial.print(pulseWidthL,DEC);
  // Serial.print("pL   ");
  // Serial.print("\n");
}

void setup(){
  pinMode(servoTU,OUTPUT);
  pinMode(servoLF,OUTPUT);
  pinMode(servoLB,OUTPUT);
  pinMode(servoRF,OUTPUT);
  pinMode(servoRB,OUTPUT);

  pinMode(sensorUS,INPUT);
  pinMode(button,INPUT);

  pinMode(13,OUTPUT);
  lastPulseL=millis();
  lastPulseR=millis();
  lastPulseTU=millis();

  nbAngle=angleMax-angleMin;
  for (int n=0;n<nbAngle;n++)
   distance[n]=100;
}

void loop(){ 
  turnOnOff=readButton();
// Serial.print(turnOnOff);
// Serial.print("  ");
 
  valIR=readSensorIR();
 
 
  setLed((100-min(max(valIR,0),100))*1024/100);
 
// Serial.print(valIR,DEC);
// Serial.print("cm   ");
  valUS=readSensorUS();
   
// Serial.print(valUS);
// Serial.print("cm   ");
 
  if(turnOnOff==1){
    pilote2(valUS,valIR);
   
   servoL(pulseWidthL);
   servoR(pulseWidthR);
   turn=servoT(getPulseT());
   if(turn==1){
    distance[angle+angleMax]=valIR;
    if(sensTU==1)
     angle=angle-1;
    else
     angle=angle+1;
   }
  }
  else{
   valPot=(valPot+readSensorPot())/2;
   servoT(valPot);
  }

  //Serial.print(valPot);
  //erial.print(" \n");
}

AWOL

If you go back to your post, click on "modify", then highlight all the code, then click on the # icon in the editor toolbar, your code will be neatly presented and legible in a proper code box, without italics.
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

PaulS

Why in heavens name are you re-implementing the Servo library?

Go Up