Go Down

Topic: Need hint, time get lost... (Read 884 times) previous topic - next topic

Roeschti

Oct 04, 2012, 08:35 am Last Edit: Oct 04, 2012, 08:39 am by Roeschti Reason: 1
Good morning!

I'm playing around with an semi-automatic robotic car. It should be controlled by bluetooth (at the end) and also follow a defined route.
I have 2 brushless motors with 2 ESC. Because the car shall start smoothly, I tried to realize start up and brake ramps linear. It means from 0 to fullspeed in 3 second. Because I'm still waiting for the ordered bluetooth shields I use serial monitor for transmitting driving commands.

My sketch works, but there's a strange behaviour of the timer3 (?): Let's say via serial monitor I transmit command 12, what means fullspeed 100% both motors in 3 seconds. The motors begin to rotate and accelerating, but it takes 7 to 8 seconds until they reach fullspeed!

But if I comment out the line intInp = serReadInt(); and just write intInp = 12; then the motors accelerate in the expectet 3 seconds.
So where does the time get lost in the function int serReadInt()?

By the way: I am beginner and still learning...

Thank you very much for all suggestions!

Code: [Select]

* Richtungsanweisungen
*    Richtung Speed Command MotorL MotorR
* Vorwärts    1        1 11 30 30
*       1        2 12 100 100
* Links       2        1 21 15 30
*       2        2 22 75 100
* Rechts      3        1 31 30 15
*       3        2 32 100 75
* Stop        9        9       99      0       0
*****************************************************************************************************************************************************/
// LIBRARIES
#include <MegaServo.h>
#include <Streaming.h>
#include <TinyGPS.h>
#include <PCD8544.h>
#include <TimerThree.h>

// PIN DEFINITIONS LCD
#define SCLK 51
#define DN 53
#define DC 49
#define SCE 45
#define RST 47

// PIN DEFINITION PIR


// PIN SERVO / BLC Regler
#define FIRST_SERVO_PIN 2
MegaServo Motor[2] ; // max servos is 48 for mega, 12 for other boards

// PIN GPS Serial1 19 RX, 18 TX

// PIN BLUETOOTH Serial2 17 RX, 16 TX

// SONSTIGES
#define iDefBaud 115200
#define iGPSBaud 4800
#define iContrast 55
#define iArmDelay 1000

// PUBLICS
int iCurSpeed1 = 0;
int iCurSpeed2 = 0;
unsigned int iCurSpeedAng1 = 0;
unsigned int iCurSpeedAng2 = 0;
int iDesSpeed1 = 0;
int iDesSpeed2 = 0;
unsigned int iReqTicks1 = 0;
unsigned int iReqTicks2 = 0;
int iAcc1 = 0;  // Beschleunigungs-Steps
int iAcc2 = 0;
unsigned int iCurTicks1 = 0;
unsigned int iCurTicks2 = 0;
const float flMaxAcc = 30.0;  //max Beschleunigung = 3 Sekunden = 30 Hunderstel wenn timer intervall auf hundertstel eingestellt ist
unsigned int intInp = 0;
unsigned int intInpOld = 0;  //letzter Befehl zwischenlagern
boolean bolNeedTm = false;    //flag ob timer benötigt wird oder nicht
long lnPrevMillis = 0;      // will store last time LED was updated
long lnInterval = 1000;          


// LCD Instanz
PCD8544 LCD = PCD8544(SCLK, DN, DC, SCE, RST);

//Serial handling
char sData[4];

void setup()
{  
 // USB Serial
 Serial.begin(iDefBaud);
 Serial.println("Enabled");

 // Motors binding, 0 = linker Motor
 Motor[0].attach( 2, 1000, 2200);
 Motor[1].attach( 3, 1000, 2200);
 // Display init
 LCD.init(iContrast);
 LCD.clear();

 //Timer init
 Timer3.initialize(100000);  //jede 100stel sekunde
 Timer3.attachInterrupt(Timer_Tick);


 //Arming ESCs
 fctLCDMsg("Regler aktivieren...",0);
 fctArmBECs(10);
 delay(1000);
 fctLCDMsg("Motoren bereit", 0);
}

void loop()
{
 intInp = serReadInt();
 if(intInp > 0)
 {
   switch (intInp)
   {
   case 11:
     //Vorwärts langsam
     iDesSpeed1 = 30;
     iDesSpeed2 = 30;
     break;
   case 12:
     //Vorwärts schnell...volles rohr falls letzer Command eine schnelle Kurve war, sonst mit rampe
     if(intInpOld == 22)  //links schnell
     {
       Motor[0].write(180);
       return;
     }
     else if(intInpOld == 32)  //rechts schnell
     {
       Motor[1].write(180);
       return;
     }
     else
     {
       iDesSpeed1 = 100;
       iDesSpeed2 = 100;
     }
     break;
   case 21:
     //Links langsam
     iDesSpeed1 = 15;
     iDesSpeed2 = 30;
     break;  
   case 22:
     //links schnell
     iDesSpeed1 = 75;
     iDesSpeed2 = 100;
     break;    
   case 31:
     //Rechts langsam
     iDesSpeed1 = 30;
     iDesSpeed2 = 15;
     break;  
   case 32:
     //Rechts schnell
     iDesSpeed1 = 100;
     iDesSpeed2 = 75;
     break;    

   default:
     //Stop
     iDesSpeed1 = 10;
     iDesSpeed2 = 10;
     break;    
   }

   //iDesSpeed1 = 100;
   //iDesSpeed2 = 100;
   iReqTicks1 = fctGetReqTicks(1);
   iReqTicks2 = fctGetReqTicks(2);
   iAcc1 = fctGetAcc(iReqTicks1, 1);
   iAcc2 = fctGetAcc(iReqTicks2, 2);
   iCurTicks1 = 0;
   iCurTicks2 = 0;
   intInpOld = intInp;
   bolNeedTm = true;  
 }
}

void RunMotors()
{

}

/**************************************************************************************************************************/

void Timer_Tick(void)
{
 if(bolNeedTm == true)
 {
   if(iCurTicks1 <= iReqTicks1)
   {
     iCurSpeed1 = iCurSpeed1 + iAcc1;
     iCurSpeedAng1 = fctGetSpeedAngle(iCurSpeed1);
     Motor[0].write(iCurSpeedAng1);  //Speed als Winkel von 0 - 180 Grad -> map    
     iCurTicks1 = iCurTicks1 + 1;
     //Serial << F("des1: ") << iDesSpeed1 << "  cur1: " << iCurSpeed1 << "  iCurTicks1:  " << iCurTicks1 << "  ReqTicks:  " << iReqTicks1
   }

   if(iCurTicks2 <= iReqTicks2)
   {
     iCurSpeed2 = iCurSpeed2 + iAcc2;
     iCurSpeedAng2 = fctGetSpeedAngle(iCurSpeed2);
     Motor[1].write(iCurSpeedAng2);  //Speed als Winkel von 0 - 180 Grad -> map  
     iCurTicks2 = iCurTicks2 + 1;
   }
   //wenn beide Ticks erreicht sind, braucht es den timer ned
   if(iCurTicks1 >= iReqTicks1 && iCurTicks2 >= iReqTicks2)
   {
     //angleichen
     iCurSpeedAng1 = fctGetSpeedAngle(iDesSpeed1);
     iCurSpeedAng2 = fctGetSpeedAngle(iDesSpeed2);
     Motor[0].write(iCurSpeedAng1);  //Speed als Winkel von 0 - 180 Grad -> map    
     Motor[1].write(iCurSpeedAng2);  //Speed als Winkel von 0 - 180 Grad -> map    
     bolNeedTm = false;
     //Serial << F("  M1 Winkel: ") << Motor[0].read() << F("  M2 Winkel: ") << Motor[1].read() << "\n";
   }
 }
 //RunMotors();
}

int fctGetReqTicks(unsigned int iMotorNr)
{
 unsigned int iSpdDiff = 0;
 //für welchen motor?
 if(iMotorNr == 1)
 {
   //beschleunigen oder bremsen?
   if(iDesSpeed1 > iCurSpeed1) //beschleunigen
   {
     iSpdDiff = iDesSpeed1 - iCurSpeed1;  //Speed Differenz ermitteln
   }
   else  //bremsen
   {
     iSpdDiff = iCurSpeed1 - iDesSpeed1;  //Speed Differenz ermitteln
   }
 }
 else
 {
   //beschleunigen oder bremsen?
   if(iDesSpeed2 > iCurSpeed2) //beschleunigen
   {
     iSpdDiff = iDesSpeed2 - iCurSpeed2;  //Speed Differenz ermitteln
   }
   else  //bremsen
   {
     iSpdDiff = iCurSpeed2 - iDesSpeed2;  //Speed Differenz ermitteln
   }
 }
 //notwendige Ticks kalkulieren bei max 3 Sekunden
 float flReqTicks = flMaxAcc / 100 * iSpdDiff;
 unsigned int iReqTicks = int(flReqTicks);
 return iReqTicks;
}

int fctGetAcc(unsigned int iTicks, unsigned int iMotorNr)
{
 //Notwendige Schrittweite für Beschleunigung berechnen, egal ob positiv oder negativ
 float flAccVal = 0.0;
 if(iMotorNr == 1)
 {
   flAccVal = iDesSpeed1 - iCurSpeed1;
   flAccVal = flAccVal / iTicks;
 }
 else
 {
   flAccVal = iDesSpeed2 - iCurSpeed2;
   flAccVal = flAccVal / iTicks;
 }
 int iAccVal = int(flAccVal);
 //Serial << F("des1: ") << iDesSpeed1 << "  cur1: " << iCurSpeed1 << "  ticks:  " << iTicks <<"\n";
 //Serial << F("AccVal: ") << iAccVal << "\n";
 return iAccVal;
}

int fctGetSpeedAngle(unsigned int intSpeed)
{
 int res = map(intSpeed, 0, 100, 0, 180);
 return res;
}

void fctLCDMsg(String strMsg, unsigned int intDelay)
{
 LCD.clear();
 LCD.print(strMsg);
 LCD.display();
 if(intDelay > 0)
 {
   delay(intDelay);
 }
}

int serReadInt()
{
 unsigned int iRes = 0;
 if (Serial.available())
 {
   //verschnaufpause
   delay(10);
   int i = 0;    
   while(i<5){
     sData[i] = Serial.read();
     //Serial.println(sData[i]);
     i++;
   }
   //aufräumen
   Serial.flush();
   //numerischen string in integer wandeln
   iRes = atoi(sData);
 }
 return iRes;
}

void fctArmBECs(int intSpeed)
{
 int iAngle = map(intSpeed, 0, 100, 0, 180);
 Motor[0].write(iAngle);
 Motor[1].write(iAngle);
 delay(iArmDelay); //delay 1 second,  some speed controllers may need longer
}


PaulS

Quote
It should be controlled by bluetooth (at the end) and also follow a defined route.

Manual control (via bluetooth) and autonomous action (following a defined route) are mutually exclusive modes.

Code: [Select]
  if(bolNeedTm == true)
Look like a real programmer, and lose the == true bit...

Code: [Select]
      iCurTicks1 = iCurTicks1 + 1;
You know, you aren't programming the Arduino using a language called C=C+1. Embrace the ++ operator, like a real programmer.

Code: [Select]
int fctGetReqTicks(unsigned int iMotorNr)
{
  <snip>
  float flReqTicks = flMaxAcc / 100 * iSpdDiff;
  unsigned int iReqTicks = int(flReqTicks);
  return iReqTicks;
}

The function says that it returns an int, yet the return statement returns an unsigned int formed from an int. Consistency is a good (even necessary) thing.

Code: [Select]

int fctGetSpeedAngle(unsigned int intSpeed)
{
  int res = map(intSpeed, 0, 100, 0, 180);
  return res;
}

This is a pretty inefficient way to multiply by 1.8. The flopping between signed and unsigned values is not a good idea.

Code: [Select]
  //RunMotors();
Is your delete key broken? You KNOW that you shouldn't be executing ANY of the commented out code in an interrupt handler. Resist the temptation to uncomment statements by using the delete key.

Code: [Select]
  if (Serial.available())
  {
    //verschnaufpause
    delay(10);
    int i = 0;     
    while(i<5){
      sData[i] = Serial.read();

If there is at least one byte to read, read all 5 of them. I'm sure you can see that this is plain wrong.

Code: [Select]
    Serial.flush();
Why? 99.99999% of the time, this statement is added by totally clueless idiots. That isn't you, is it?

Code: [Select]
    iRes = atoi(sData);
Wrong! The atoi() function expects a NULL terminated array of chars. You're array is NOT NULL terminated.

AWOL

Quote
You're array is NOT NULL terminated.

Look like a real English speaker, and use "your"  ;)
"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

Quote
Look like a real English speaker, and use "your"

I never claimed to be an English teacher, but point taken.

Roeschti


Quote
It should be controlled by bluetooth (at the end) and also follow a defined route.

Manual control (via bluetooth) and autonomous action (following a defined route) are mutually exclusive modes.

Wow, really? After loosing the bluetooth connection it runs home.


Code: [Select]
  if(bolNeedTm == true)
Look like a real programmer, and lose the == true bit...

That's true.

Code: [Select]
      iCurTicks1 = iCurTicks1 + 1;
You know, you aren't programming the Arduino using a language called C=C+1. Embrace the ++ operator, like a real programmer.

Oh, thanks for that hint. I thought it's C#, but C# = C# + 1 doesn't work...

Code: [Select]
int fctGetReqTicks(unsigned int iMotorNr)
{
  <snip>
  float flReqTicks = flMaxAcc / 100 * iSpdDiff;
  unsigned int iReqTicks = int(flReqTicks);
  return iReqTicks;
}

The function says that it returns an int, yet the return statement returns an unsigned int formed from an int. Consistency is a good (even necessary) thing.

Yes, you're (oh pardon, you are) right, unsigned is better, I don't like negative stuff.

Code: [Select]

int fctGetSpeedAngle(unsigned int intSpeed)
{
  int res = map(intSpeed, 0, 100, 0, 180);
  return res;
}

This is a pretty inefficient way to multiply by 1.8. The flopping between signed and unsigned values is not a good idea.

This is true again, I really should not copy published code from this forum and use it. I see I am a bad boy  :smiley-red:

Code: [Select]
  //RunMotors();
Is your delete key broken? You KNOW that you shouldn't be executing ANY of the commented out code in an interrupt handler. Resist the temptation to uncomment statements by using the delete key.

Oh, I don't have such a key.. you mean the Del key maybe? This political correctness is hard to handle.

Code: [Select]
  if (Serial.available())
  {
    //verschnaufpause
    delay(10);
    int i = 0;     
    while(i<5){
      sData[i] = Serial.read();

If there is at least one byte to read, read all 5 of them. I'm sure you can see that this is plain wrong.

Yeah I learnd this from the americans! You know, bigger is better! Why drive a Prius if you can drive a fat, inefficient V8 big block?

Code: [Select]
    Serial.flush();
Why? 99.99999% of the time, this statement is added by totally clueless idiots. That isn't you, is it?
Hmmm...good question. Well then the clueless idiots should not post such things...beginners like me believe what Pros tell'em

Code: [Select]
    iRes = atoi(sData);
Wrong! The atoi() function expects a NULL terminated array of chars. You're array is NOT NULL terminated.
This may be, works anyway perfect.


PaulS

Quote
This may be, works anyway perfect.

Being lucky is good. Being correct is even better.

Emmeran

Maybe the delay(10) which delays your main loop for 10ms whenever there's serial data. Depends on your serial traffic, but I don't see anything else that would be as obvious.

BTW: While PaulS is kinda talking down to you, you're still the one asking for help and even though he's not helping you with the question you asked, he still gave you some valuable advice.
So if you don't want to discourage other people to help you, maybe show a bit less attitude?

Roeschti

This was exactly the point, thanks! I found a solution here: http://arduino.cc/forum/index.php?PHPSESSID=4e071436c045990f70a6d5a12bb01afd&/topic,99225.0.html

BTW: well, why should I? It's very simple. I asked for a hint (and not for the entire solution) and wrote I'm beginner. So if somebody is friendly to me, I'm friendly too and vise versa. How do you say that in english, tit for tat? Asking people whether they are clueless idiots shows enough about the character of such people. Anyway, it's not a problem for me because I don't care about such people.


Go Up