260 lines
5.1 KiB
C++
260 lines
5.1 KiB
C++
|
/******************************************************************************
|
|||
|
* The MIT License
|
|||
|
*
|
|||
|
* Copyright (c) 2015 Frank-Michael Krause
|
|||
|
*
|
|||
|
* Permission is hereby granted, free of charge, to any person
|
|||
|
* obtaining a copy of this software and associated documentation
|
|||
|
* files (the "Software"), to deal in the Software without
|
|||
|
* restriction, including without limitation the rights to use, copy,
|
|||
|
* modify, merge, publish, distribute, sublicense, and/or sell copies
|
|||
|
* of the Software, and to permit persons to whom the Software is
|
|||
|
* furnished to do so, subject to the following conditions:
|
|||
|
*
|
|||
|
* The above copyright notice and this permission notice shall be
|
|||
|
* included in all copies or substantial portions of the Software.
|
|||
|
*
|
|||
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
|||
|
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
|
|||
|
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
|||
|
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
|
|||
|
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
|
|||
|
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
|
|||
|
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|||
|
* SOFTWARE.
|
|||
|
*****************************************************************************/
|
|||
|
#include <wirish/wirish.h>
|
|||
|
#include "motor.h"
|
|||
|
#include "../gyro/gyro.h"
|
|||
|
#include "debug.h"
|
|||
|
|
|||
|
|
|||
|
#define TB6612
|
|||
|
|
|||
|
#define MOTOR_ANZ 3
|
|||
|
|
|||
|
|
|||
|
typedef struct {
|
|||
|
int dir;
|
|||
|
int pwm;
|
|||
|
int sensor;
|
|||
|
int led;
|
|||
|
} MotorPins_t;
|
|||
|
|
|||
|
static MotorPins_t mpins[MOTOR_ANZ];
|
|||
|
|
|||
|
#define IDX_A 0
|
|||
|
#define IDX_B 1
|
|||
|
#define IDX_C 2
|
|||
|
|
|||
|
// Umwandlung MotorIds to Index, wenn nur ein Motor addressiert ist
|
|||
|
#define MIDX(id) ((id&OUT_A)?IDX_A:(id&OUT_B)?IDX_B:IDX_C)
|
|||
|
|
|||
|
|
|||
|
static volatile int RadCount0;
|
|||
|
void CountRad0()
|
|||
|
{
|
|||
|
RadCount0++;
|
|||
|
}
|
|||
|
|
|||
|
static volatile int RadCount1;
|
|||
|
void CountRad1()
|
|||
|
{
|
|||
|
RadCount1++;
|
|||
|
}
|
|||
|
|
|||
|
static volatile int RadCount2;
|
|||
|
void CountRad2()
|
|||
|
{
|
|||
|
RadCount2++;
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void InitMotor(int id, int PinDir, int PinPWM, int PinSensor, int PinLED)
|
|||
|
{
|
|||
|
MotorPins_t *p = mpins+MIDX(id);
|
|||
|
p->dir = PinDir;
|
|||
|
p->pwm = PinPWM;
|
|||
|
p->sensor = PinSensor;
|
|||
|
p->led = PinLED;
|
|||
|
|
|||
|
pinMode (p->pwm, PWM);
|
|||
|
pinMode (p->dir, OUTPUT);
|
|||
|
analogWrite(p->pwm, 0);
|
|||
|
|
|||
|
if (p->sensor != -1)
|
|||
|
{
|
|||
|
if (p->led != -1)
|
|||
|
pinMode(p->led, OUTPUT);
|
|||
|
|
|||
|
pinMode(p->sensor, INPUT);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
static inline void RadSensorAn(int motor)
|
|||
|
{
|
|||
|
MotorPins_t *p = mpins;
|
|||
|
|
|||
|
for (int idx=0; idx<MOTOR_ANZ; idx++)
|
|||
|
{
|
|||
|
if (motor & (1<<idx)) {
|
|||
|
if (p->led != -1)
|
|||
|
digitalWrite(p->led, HIGH);
|
|||
|
if (p->sensor != -1) {
|
|||
|
if (idx == 0)
|
|||
|
attachInterrupt(p->sensor, CountRad0, CHANGE);
|
|||
|
else if (idx == 1)
|
|||
|
attachInterrupt(p->sensor, CountRad1, CHANGE);
|
|||
|
else
|
|||
|
attachInterrupt(p->sensor, CountRad2, CHANGE);
|
|||
|
}
|
|||
|
}
|
|||
|
p++;
|
|||
|
}
|
|||
|
delayMicroseconds(50); // Damit ggf. ein Interrupt beim Einschalten ausgef<65>hrt wird
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
static inline void RadSensoren(int motor)
|
|||
|
{
|
|||
|
MotorPins_t *p = mpins;
|
|||
|
|
|||
|
for (int idx=0; idx<MOTOR_ANZ; idx++)
|
|||
|
{
|
|||
|
if (motor & (1<<idx)) {
|
|||
|
if (p->led != -1)
|
|||
|
digitalWrite(p->led, LOW);
|
|||
|
if (p->sensor != -1) {
|
|||
|
detachInterrupt(p->sensor);
|
|||
|
}
|
|||
|
}
|
|||
|
p++;
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void OnFwd(int Motor, int Geschwindigkeit)
|
|||
|
{
|
|||
|
if(Geschwindigkeit>0)
|
|||
|
Geschwindigkeit+=13;
|
|||
|
else if(Geschwindigkeit<0)
|
|||
|
Geschwindigkeit-=13;
|
|||
|
|
|||
|
MotorPins_t *p = mpins;
|
|||
|
Geschwindigkeit = constrain(Geschwindigkeit,-100, 100); // Geschwindigkeit auf Bereich -100 ... +100 begrenzen
|
|||
|
Geschwindigkeit = (65535*Geschwindigkeit)/100;
|
|||
|
PrintNumber("v ",Geschwindigkeit );
|
|||
|
|
|||
|
for (int i=0; i<MOTOR_ANZ; i++)
|
|||
|
{
|
|||
|
if (Motor & (1<<i))
|
|||
|
{
|
|||
|
#ifdef TB6612
|
|||
|
int v;
|
|||
|
if (Geschwindigkeit>0)
|
|||
|
{
|
|||
|
digitalWrite(p->dir, HIGH);
|
|||
|
v = Geschwindigkeit;
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
digitalWrite(p->dir, LOW);
|
|||
|
v = -Geschwindigkeit;
|
|||
|
}
|
|||
|
analogWrite(p->pwm, v);
|
|||
|
#endif
|
|||
|
}
|
|||
|
p++;
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void OnRev(int m, int v)
|
|||
|
{
|
|||
|
OnFwd(m, -v);
|
|||
|
}
|
|||
|
|
|||
|
void Off(int Motor)
|
|||
|
{
|
|||
|
MotorPins_t *p = mpins;
|
|||
|
for (int i=0; i<MOTOR_ANZ; i++)
|
|||
|
{
|
|||
|
if (Motor & (1<<i))
|
|||
|
{
|
|||
|
#ifdef TB6612
|
|||
|
analogWrite(p->pwm, 0);
|
|||
|
#endif
|
|||
|
}
|
|||
|
p++;
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
|
|||
|
#if 0
|
|||
|
|
|||
|
// gyro-Funktionen nicht in diesem Modul
|
|||
|
|
|||
|
void DreheRechts(int v, int Winkel)
|
|||
|
{
|
|||
|
gyroResetWinkel();
|
|||
|
OnFwd(OUT_A, v);
|
|||
|
OnFwd(OUT_B, -v);
|
|||
|
while (winkelz>=-Winkel)
|
|||
|
gyroUpdate();
|
|||
|
Off (OUT_AB);
|
|||
|
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void DreheLinks(int v, int Winkel)
|
|||
|
{
|
|||
|
gyroResetWinkel();
|
|||
|
OnFwd(OUT_A, -v);
|
|||
|
OnFwd(OUT_B, v);
|
|||
|
while (winkelz<=Winkel)
|
|||
|
gyroUpdate();
|
|||
|
Off (OUT_AB);
|
|||
|
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void FahreVor(int v, int Strecke)
|
|||
|
{
|
|||
|
RadSensorenAn();
|
|||
|
RadCountR = 0; // 0 Setzen erst NACH Einschalten des Sensors, eventuell kommt schon ein Interrupt
|
|||
|
// RadCountL = 0;
|
|||
|
OnFwd(OUT_AB, v);
|
|||
|
while (Strecke>RadCountR)
|
|||
|
{
|
|||
|
// PrintNumber("RC", RadCountR);
|
|||
|
// delay (1);
|
|||
|
}
|
|||
|
Off(OUT_AB);
|
|||
|
RadSensorenAus();
|
|||
|
}
|
|||
|
|
|||
|
#endif
|
|||
|
|
|||
|
|
|||
|
#if 0
|
|||
|
void TestRadSensoren()
|
|||
|
{
|
|||
|
RadSensorenAn();
|
|||
|
RadCountR = 0;
|
|||
|
RadCountL = 0;
|
|||
|
OnFwd(OUT_AB,10);
|
|||
|
while(1)
|
|||
|
{
|
|||
|
lcd.clear();
|
|||
|
lcd.setCursor(0,0);
|
|||
|
lcd.print(RadCountL);
|
|||
|
lcd.setCursor(4,0);
|
|||
|
lcd.print(RadCountR);
|
|||
|
delay(300);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
#endif
|