Pages: [1]   Go Down
Author Topic: sweep servo not working ???  (Read 429 times)
0 Members and 1 Guest are viewing this topic.
Australia
Offline Offline
Newbie
*
Karma: 0
Posts: 5
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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");
}
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 309
Posts: 26489
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

"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.

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 634
Posts: 50243
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Pages: [1]   Go Up
Jump to: