Pages: [1]   Go Down
Author Topic: Need hint, time get lost...  (Read 745 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 32
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
* 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
}

« Last Edit: October 04, 2012, 01:39:36 am by Roeschti » Logged

Seattle, WA USA
Online Online
Brattain Member
*****
Karma: 551
Posts: 46242
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
  if(bolNeedTm == true)
Look like a real programmer, and lose the == true bit...

Code:
      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:
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:
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:
  //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:
  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:
    Serial.flush();
Why? 99.99999% of the time, this statement is added by totally clueless idiots. That isn't you, is it?

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

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

Quote
You're array is NOT NULL terminated.
Look like a real English speaker, and use "your"  smiley-wink
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
Online Online
Brattain Member
*****
Karma: 551
Posts: 46242
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
Look like a real English speaker, and use "your"
I never claimed to be an English teacher, but point taken.
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 32
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
  if(bolNeedTm == true)
Look like a real programmer, and lose the == true bit...

That's true.

Code:
      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:
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:
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:
  //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:
  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:
    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:
    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.

Logged

Seattle, WA USA
Online Online
Brattain Member
*****
Karma: 551
Posts: 46242
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
This may be, works anyway perfect.
Being lucky is good. Being correct is even better.
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 26
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Offline Offline
Newbie
*
Karma: 0
Posts: 32
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.

Logged

Pages: [1]   Go Up
Jump to: