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
|