Morgen allerseits!
Bastle eine Art Roboterwagen, welcher mit 2 Brushless Motoren angetrieben wird, welche linear beschleunigt und auch abgebremst werden. Habe ein Mega Board und dazu auch die MegaServo Lib verwendet. Die Sache funktioniert am Ende via Bluetooth dh bekommt von einem Androiden über die Serial Fahrkommandos. Da die BT Sachen noch nicht da sind, übermittle ich zZ die Kommandos via den Serial Monitor. Es funktioniert im Moment exakt so, wie ich mir das vorstellte.
Der Code dazu:
// 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;
float flMaxAcc = 30.0; //max Beschleunigung = 3 Sekunden = 30 Hunderstel wenn timer intervall auf hundertstel eingestellt ist
unsigned int intInp = 0;
// 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, 800, 2200);
Motor[1].attach( 3, 800, 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(0);
fctLCDMsg("Motor1 bereit", 0);
delay(2000);
}
void loop()
{
intInp = serReadInt();
if(intInp > 0)
{
switch (intInp) {
case 11:
//Vorwärts langsam
iDesSpeed1 = 30;
iDesSpeed2 = 30;
break;
case 12:
//Vorwärts schnell
iDesSpeed1 = 100;
iDesSpeed2 = 100;
break;
case 21:
//Links langsam
iDesSpeed1 = 0;
iDesSpeed2 = 30;
break;
case 22:
//links schnell
iDesSpeed1 = 30;
iDesSpeed2 = 100;
break;
case 31:
//Rechts langsam
iDesSpeed1 = 30;
iDesSpeed2 = 0;
break;
case 32:
//Rechts schnell
iDesSpeed1 = 100;
iDesSpeed2 = 30;
break;
default:
//Stop
iDesSpeed1 = 0;
iDesSpeed2 = 0;
break;
}
iReqTicks1 = fctGetReqTicks(1);
iReqTicks2 = fctGetReqTicks(2);
iAcc1 = fctGetAcc(iReqTicks1, 1);
iAcc2 = fctGetAcc(iReqTicks2, 2);
iCurTicks1 = 0;
iCurTicks2 = 0;
//Serial.print("Angekommen: ");
//Serial.println(iDesSpeed1);
}
RunMotors();
}
void RunMotors()
{
Motor[0].write(iCurSpeedAng1); //Speed als Winkel von 0 - 180 Grad -> map
Motor[1].write(iCurSpeedAng2); //Speed als Winkel von 0 - 180 Grad -> map
}
/**************************************************************************************************************************/
void Timer_Tick(void)
{
if(iCurTicks1 <= iReqTicks1)
{
iCurSpeed1 = iCurSpeed1 + iAcc1;
iCurSpeedAng1 = fctGetSpeedAngle(iCurSpeed1);
iCurTicks1 = iCurTicks1 + 1;
}
if(iCurTicks2 <= iReqTicks2)
{
iCurSpeed2 = iCurSpeed2 + iAcc2;
iCurSpeedAng2 = fctGetSpeedAngle(iCurSpeed2);
iCurTicks2 = iCurTicks2 + 1;
}
}
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()
{
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
int intRes = atoi(sData);
return intRes;
}
else
{
return 0;
}
}
void fctArmBECs(int intSpeed)
{
int iAngle = map(intSpeed, 0, 100, 0, 180);
Motor[0].write(iAngle);
delay(iArmDelay);
Motor[1].write(iAngle);
delay(iArmDelay); //delay 1 second, some speed controllers may need longer
}
Im Main Loop werden nun pausenlos die Motoren dh die Regler "beballert" mit Run Kommandos, auch wenn kein neues
Fahrkommando anliegt. Ist dies notwendig? Ich weiss nicht, wie in der RC Welt die Funkverbindung geht, ob da pausenlos auch Servo Kommandos übermittelt werden, oder nur wenn eine Bewegung notwendig ist. Kennt sich da vielleicht jemand aus? Ich google mir die Finger wund, weiss aber nicht recht wonach ich suchen soll...
Besten Dank für alle Tips!