Australia
 OfflineNewbie
Karma: 0
Posts: 5
|
 |
« on: October 04, 2011, 10:25:13 pm » |
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");
}
|
