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