void ScanObstacle(){
Neck.write(Neckcenter);
chThdSleepMilliseconds(100);
CheckDistance();
if (obstacleDistance > 20){ //no obstacle nearby
Obstacle=0;
chMtxLock(&serialMutex);
Serial.print(obstacleDistance);
Serial.print("cm center over 20");
Serial.println();
chMtxUnlock();
}
if (obstacleDistance <= 20){ //check sensor
BuzzerBeep();
Neck.write(Neckcenter);
chThdSleepMilliseconds(100);
digitalWrite(Red, HIGH);
CheckDistance();
chThdSleepMilliseconds(10);
obstacleCenter = obstacleDistance;
Neck.write(Neckcenter+30); //turn head left
chThdSleepMilliseconds(200);
digitalWrite(Green, HIGH);
CheckDistance();
obstacleLeft = obstacleDistance;
digitalWrite(Green, LOW);
Neck.write(Neckcenter-30); //turn head right
chThdSleepMilliseconds(200);
digitalWrite(Blue, HIGH);
CheckDistance();
obstacleRight = obstacleDistance;
digitalWrite(Blue, LOW);
Neck.write(Neckcenter);
if ((obstacleLeft <= obstacleAhead) && (obstacleRight >= obstacleLeft)){
Obstacle=1;
}
if ((obstacleRight <= obstacleAhead) && (obstacleLeft >= obstacleRight)){
Obstacle=2;
}
if (((obstacleLeft <= obstacleAhead && obstacleRight <= obstacleAhead && obstacleCenter <= obstacleAhead) && (obstacleCenter == obstacleLeft && obstacleCenter == obstacleRight)) || (obstacleLeft <= obstacleWarning && obstacleRight <= obstacleWarning && obstacleCenter <= obstacleWarning)){
Obstacle=3;
}
if ((obstacleLeft <= obstacleAlert) || (obstacleRight <= obstacleAlert) || (obstacleCenter <= obstacleAlert)) {
Obstacle=4;
}
}
chMtxLock(&serialMutex);
Serial.print(obstacleCenter);
Serial.print("cm center");
Serial.println();
Serial.print(obstacleLeft);
Serial.print("cm left");
Serial.println();
Serial.print(obstacleRight);
Serial.print("cm right");
Serial.println();
Serial.print(Obstacle);
Serial.print(" Case");
Serial.println();
Serial.println("Memory use");
Serial.println("Area,Size,Unused");
Serial.print("Thread 1,");
// size of stack for thread 1
Serial.print(sizeof(waThread1) - sizeof(Thread));
Serial.write(',');
// unused stack for thread 1
Serial.println(chUnusedStack(waThread1, sizeof(waThread1)));
Serial.print("Thread 2,");
// size of stack for thread 2
Serial.print(sizeof(waThread2) - sizeof(Thread));
Serial.write(',');
// unused stack for thread 2
Serial.println(chUnusedStack(waThread2, sizeof(waThread2)));
Serial.print("Thread 3,");
// size of stack for thread 3
Serial.print(sizeof(waThread3) - sizeof(Thread));
Serial.write(',');
// unused stack for thread 3
Serial.println(chUnusedStack(waThread3, sizeof(waThread3)));
// print stats for heap/main thread area
Serial.print("Heap/Main,");
Serial.print(chHeapMainSize());
Serial.print(",");
Serial.println(chUnusedHeapMain());
// end task
chMtxUnlock();
}
void CheckDistance(){
// establish variables for duration of the ping,
// and the distance result in inches and centimeters:
long duration, cm;
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
chThdSleepMilliseconds(2);
digitalWrite(pingPin, HIGH);
chThdSleepMilliseconds(5);
digitalWrite(pingPin, LOW);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
// convert the time into a distance
chThdSleepMilliseconds(10);
cm = microsecondsToCentimeters(duration);
obstacleDistance = cm;
}
long microsecondsToCentimeters(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}
void WalkDirection(){
noInterrupts();
Serial.print(Obstacle);
Serial.print(" Case");
Serial.println();
walkToggle = Obstacle;
interrupts();
switch (walkToggle){
case 0: //no object
digitalWrite(Green, HIGH);
digitalWrite(Red, HIGH);
Forward(1,30); //one step Forward
digitalWrite(Green, LOW);
digitalWrite(Red, LOW);
break;
case 1: //object on Left
digitalWrite(Green, HIGH);
TurnRight(2,30);
digitalWrite(Green, LOW);
break;
case 2: //object on Right
digitalWrite(Blue, HIGH);
TurnLeft(2,30);
digitalWrite(Blue, LOW);
break;
case 3: //obect in Front (both Left and Right detect the object)
digitalWrite(Red, HIGH);
TurnLeft(4,30); //turn around
digitalWrite(Red, LOW);
break;
case 4: //obect in Front (both Left and Right detect the object)
digitalWrite(Red, HIGH);
Reverse(2,30); //turn around
digitalWrite(Red, LOW);
break;
}
}
So I guess I was scratching my head for 15 hours because I need a second power source for the servos...
6 servos, 2 RGB lights an Arduino Micro and a Ping Sensor cant run on one 9v 170mAh battery, plus I learned the ins and outs of ChibiOS
Thanks for the help
My serial output with the battery and USB hooked up looks like this:
24cm center
22cm left
0cm right
4 Case
28cm center
29cm left
0cm right
4 Case
43cm center
62cm left
39cm right
6cm center
4 Case
0cm left
39cm right
4 Case
38cm center
16cm left
0cm right
4 Case
40cm center
0cm left
41cm right
80cm center over 20
59cm center over 20
42cm center over 20
40cm center over 20
0 Case
0 Case
0cm center
4cm left
69cm right
4 Case
0cm center
244cm left
70cm right
4 Case
220cm center
245cm left
69cm right
79cm center
246cm left
69cm right
210cm center over 20
0 Case
22cm center
246cm left
63cm right
0 Case
0 Case
It's still missing some sensor readings, although not all the time. Maybe i need to use a semaphore to get the two processes synchronized? I was moving the bot around during that to get different readings and the reported distances are accurate.
bytedisorder:
Or it could also be that while my 9v 170mAh rechargeable is providing enough current to drive 1 servo and the ping, 5 servos or 6 servos (and 2 RGB LEDs... It is not providing enough current to run 6 servos, the ping and 2RGB LEDs concurrently
You can hardly use that battery for running that stuff.. The above battery is intended for powering systems with 20mA current consumption. A single servo can take 500mA or more when acting.
Better you use for example a 7.4V li-poly rechargeable battery pack..
What about a 9v li-po rechargable at 500mAh?
500mAh - that is the capacity (C). The Q is how much current it can deliver (in a much shorter time of course).
Li-po batteries can do 1C up to 60C current (C=capacity) afaik, based on type etc. If the li-po pack is 500mAh and for example its C=10, it can deliver ~5A for ~6minutes..
I am using 9 gram HTX900 servos which at peak have 1.6kg of torque but at no time are they lifting more then 350 grams so I doubt they are using the 500 mA each, I know that wiring lithium batteries in parallel is supposed to be a no-no
I know we are off topic for software now!
This thing was previously running on a single nickel rechargeable 170mAh and I think that is why with 5 servos and the sensor reading were fine but with servo six things got inconsistent because it could not supply the current even at full charge.