Commit 3ddfe3a0 authored by springob's avatar springob

* bob3 arduino library


git-svn-id: svn://svn.code.sf.net/p/nibo/code/niborobolib/trunk/arduino16/libraries/BOB3/src@312 2d339c8f-7935-0410-b692-f2b57624aac1
parents
#ifdef ARDUINO
#include "bob3/iodefs.h"
#include "bob3/analog.h"
#include "bob3/leds.h"
#include "BOB3.h"
#include "bob3/utils.h"
#include "bob3/ircom.h"
#else
#include "iodefs.h"
#include "analog.h"
#include "leds.h"
#include "bob3.h"
#include "utils.h"
#include "ircom.h"
#endif
#include <stdlib.h>
#include <avr/io.h>
#include <avr/boot.h>
#ifndef SIGRD
#define SIGRD 5
#endif
Bob3 bob3;
uint8_t _bob3_revision;
void Bob3::init() {
uint8_t sig2 = boot_signature_byte_get(4);
if (sig2==0x0a) _bob3_revision = 102; // V1.02
if (sig2==0x0f) _bob3_revision = 103; // V1.03
leds_init();
analog_init();
leds_set_RGBx(1, OFF);
leds_set_RGBx(2, OFF);
}
void Bob3::setLed(uint8_t id, uint16_t color) {
switch (id) {
case 1:
case 2:
leds_set_RGBx(id, color);
break;
case 3:
set_output_bitval(IO_LED_1, color);
break;
case 4:
set_output_bitval(IO_LED_2, color);
break;
}
}
uint16_t Bob3::getLed(uint8_t id) {
if (id<=2) {
return leds_get_RGBx(id);
} else if (id==3) {
return get_output_bit(IO_LED_1)?ON:OFF;
} else if (id==4) {
return get_output_bit(IO_LED_2)?ON:OFF;
}
return 0;
}
uint16_t Bob3::getIRSensor() {
return analog_getValueExt(ANALOG_IR, 2);
}
void Bob3::enableIRSensor(bool enable) {
}
uint8_t Bob3::getArm(uint8_t id) {
uint8_t v0, v1, v2, v3;
if (id==1) {
v0 = analog_getValueExt(ANALOG_L0, 0);
v1 = analog_getValueExt(ANALOG_L1, 0);
v2 = analog_getValueExt(ANALOG_L2, 0);
v3 = analog_getValueExt(ANALOG_L3, 0);
} else if (id==2) {
v0 = analog_getValueExt(ANALOG_R0, 0);
v1 = analog_getValueExt(ANALOG_R1, 0);
v2 = analog_getValueExt(ANALOG_R2, 0);
v3 = analog_getValueExt(ANALOG_R3, 0);
} else {
return 0;
}
if (v0&0x01) return 1;
if (v1&0x01) return 2;
if (v2&0x01) return 2;
if (v3&0x01) return 3;
/*
v = max(v0, v1, v2, v3);
//if (v<175) {
if (v>80) {
if (v==v0) {
return 3;
} else if (v==v1) {
return 2;
} else if (v==v2) {
return 2;
} else if (v==v3) {
return 1;
}
}
*/
return 0;
}
void Bob3::enableArms(bool enable) {
}
uint16_t Bob3::getTemperature() {
return analog_getValueExt(ANALOG_TEMP, 0);
}
uint16_t Bob3::getMillivolt() {
// ADC_BANDGAP_CHANNEL_VOLTAGE must be below 1.28 V!!!
uint16_t voltage = analog_getValueExt(ANALOG_VOLT, 0);
voltage = ((uint16_t)(50*ADC_BANDGAP_CHANNEL_VOLTAGE*1024))/voltage;
voltage *= 20;
return voltage;
}
uint8_t Bob3::getID() {
uint8_t id=0;
deactivate_output_bit(IO_ID_0);
deactivate_output_bit(IO_ID_1);
deactivate_output_bit(IO_ID_2);
deactivate_output_bit(IO_ID_3);
deactivate_output_bit(IO_ID_4);
set_output_bit(IO_ID_0);
set_output_bit(IO_ID_1);
set_output_bit(IO_ID_2);
set_output_bit(IO_ID_3);
set_output_bit(IO_ID_4);
id += get_input_bit(IO_ID_0)?0:1;
id += get_input_bit(IO_ID_1)?0:2;
id += get_input_bit(IO_ID_2)?0:4;
id += get_input_bit(IO_ID_3)?0:8;
id += get_input_bit(IO_ID_4)?0:16;
clear_output_bit(IO_ID_0);
clear_output_bit(IO_ID_1);
clear_output_bit(IO_ID_2);
clear_output_bit(IO_ID_3);
clear_output_bit(IO_ID_4);
return id;
}
int16_t Bob3::receiveIRCode(uint8_t carrier, uint16_t timeout) {
int16_t code;
uint8_t an_bak = analog_enable(false);
code = ircom_receive(carrier, timeout);
analog_enable(an_bak);
return code;
}
void Bob3::transmitIRCode(uint8_t carrier, uint8_t code) {
uint8_t an_bak = analog_enable(false);
ircom_transmit(carrier, code);
analog_enable(an_bak);
}
static uint16_t mixColor16(uint16_t color1, uint16_t color2, uint8_t w1, uint8_t w2) {
uint8_t r = (((uint8_t(color1>>8))&0x0f)*w1 + ((uint8_t(color2>>8))&0x0f)*w2)/16;
uint8_t g = (((uint8_t(color1>>4))&0x0f)*w1 + ((uint8_t(color2>>4))&0x0f)*w2)/16;
uint8_t b = (((uint8_t(color1>>0))&0x0f)*w1 + ((uint8_t(color2>>0))&0x0f)*w2)/16;
return rgb(r, g, b);
}
uint16_t mixColor(uint16_t color1, uint16_t color2, uint8_t w1, uint8_t w2) {
if (w1==0) return color2;
if (w2==0) return color1;
uint8_t sum=w1+w2;
if (sum==16) return mixColor16(color1, color2, w1, w2);
uint8_t r = (((color1>>8)&0x0f)*w1 + ((color2>>8)&0x0f)*w2)/sum;
uint8_t g = (((color1>>4)&0x0f)*w1 + ((color2>>4)&0x0f)*w2)/sum;
uint8_t b = (((color1>>0)&0x0f)*w1 + ((color2>>0)&0x0f)*w2)/sum;
return rgb(r, g, b);
}
uint16_t hsvx(uint8_t h, uint8_t _s, uint8_t _v) {
if (_s == 0) {
_v >>= 4;
return rgb(_v,_v,_v);
}
uint16_t v = _v;
uint16_t s = _s;
uint8_t seg, delta;
if (h>=213) { seg=5; delta = 6*(h-213); }
else if (h>=170) { seg=4; delta = 6*(h-170); }
else if (h>=128) { seg=3; delta = 6*(h-128); }
else if (h>=85) { seg=2; delta = 6*(h-85); }
else if (h>=42) { seg=1; delta = 6*(h-42); }
else { seg=0; delta = 6*(h-0); }
uint8_t p = (v * (255 - s)) >> 12;
uint8_t q = (v * (255 - ((s * delta) >> 8))) >> 12;
uint8_t t = (v * (255 - ((s * (255 - delta)) >> 8))) >> 12;
v >>= 4;
switch (seg) {
case 0: return rgb(v, t, p);
case 1: return rgb(q, v, p);
case 2: return rgb(p, v, t);
case 3: return rgb(p, q, v);
case 4: return rgb(t, p, v);
case 5: return rgb(v, p, q);
}
return 0;
}
uint16_t hsv(int16_t _h, uint8_t _s, uint8_t _v) {
// _h: [0..359] degree (tolarant)
// _s: [0..100] percent
// _v: [0..100] percent
if (_s == 0) {
_v = 2*_v + _v/2; // Range: [0...250]
_v >>= 4;
return rgb(_v,_v,_v);
}
uint16_t v = 2*_v + _v/2; // Range: [0...250]
uint16_t s = 2*_s + _s/2; // Range: [0...250]
uint8_t seg, delta;
while (_h<0) _h += 360;
while (_h>=360) _h -= 360;
if (_h>=300) { seg=5; delta = _h-300; }
else if (_h>=240) { seg=4; delta = _h-240; }
else if (_h>=180) { seg=3; delta = _h-180; }
else if (_h>=120) { seg=2; delta = _h-120; }
else if (_h>=60) { seg=1; delta = _h-60; }
else { seg=0; delta = _h-0; }
delta = 4*delta + delta/4; // Range: [0...250]
uint8_t p = (v * (255 - s)) >> 12;
uint8_t q = (v * (255 - ((s * delta) >> 8))) >> 12;
uint8_t t = (v * (255 - ((s * (255 - delta)) >> 8))) >> 12;
v >>= 4;
switch (seg) {
case 0: return rgb(v, t, p);
case 1: return rgb(q, v, p);
case 2: return rgb(p, v, t);
case 3: return rgb(p, q, v);
case 4: return rgb(t, p, v);
case 5: return rgb(v, p, q);
}
return 0;
}
uint32_t random32() {
static unsigned long ctx = 1;
ctx += analog_getRandomSeed();
return random_r(&ctx);
}
uint16_t randomNumber(uint16_t lo, uint16_t hi) {
uint16_t x = random16();
if ((lo==0x0000) && (hi==0xffff)) return x;
if (hi<lo) return 0;
uint16_t range = hi-lo+1;
x = x%range;
return x+lo;
}
uint16_t randomBits(uint8_t zeros, uint8_t ones) {
uint16_t val = 0;
uint8_t rounds = zeros+ones;
while (rounds--) {
val<<=1;
if (randomNumber(0, rounds) < ones) {
ones--;
val |= 0x01;
}
}
return val;
}
#ifndef _BOB3_H_
#define _BOB3_H_
#if (defined (ARDUINO)) && (!defined (ARDUINO_AVR_BOB3))
#error "BOB3 library should be used with a BOB3 robot as board!"
#endif
#include <avr/io.h>
#include <util/delay.h>
#include <bob3/utils.h>
void setup();
void loop();
enum {
ARM_NONE,
ARM_TOP,
ARM_MID_TOP,
ARM_MID_BOT,
ARM_BOT
};
enum {
OFF = 0x000,
WHITE = 0xfff,
ON = 0xfff,
RED = 0xf00,
GREEN = 0x0f0,
BLUE = 0x00f,
YELLOW = 0xff0,
CYAN = 0x0ff,
FUCHSIA = 0xf0f,
ORANGE = 0xf80,
KABARED = 0xd42,
PURPLE = 0xf08,
VIOLET = 0x63a,
AQUAMARINE = 0x7fd,
BROWN = 0xa33,
CORAL = 0xf75,
CORNFLOWERBLUE = 0x69e,
STEELBLUE = 0x48a,
ROYALBLUE = 0x46e,
FORESTGREEN = 0x282,
SEAGREEN = 0x5f9
};
enum {
ARM_1 = 1,
ARM_2 = 2,
EYE_1 = 1,
EYE_2 = 2,
LED_3 = 3,
LED_4 = 4
};
class Bob3 {
public:
void init();
void setLed(uint8_t id, uint16_t color);
uint16_t getLed(uint8_t id);
void setEyes(uint16_t eye1, uint16_t eye2) {setLed(1, eye1); setLed(2, eye2);};
void setWhiteLeds(uint16_t wled1, uint16_t wled2) {setLed(3, wled1); setLed(4, wled2);};
uint16_t getIRSensor();
void enableIRSensor(bool enable);
uint8_t getArm(uint8_t id);
void enableArms(bool enable);
uint16_t getTemperature();
uint16_t getMillivolt();
uint8_t getID();
int16_t receiveIRCode(uint8_t carrier, uint16_t timeout);
void transmitIRCode(uint8_t carrier, uint8_t code);
int16_t receiveIRCode(uint16_t timeout) {return receiveIRCode(0, timeout);}
void transmitIRCode(uint8_t code) {transmitIRCode(0, code);}
};
extern Bob3 bob3;
#if !defined(ARDUINO)
inline static
void delay(uint16_t ms) {while (ms--) _delay_ms(1);}
#endif
inline static uint16_t rgb(uint8_t r, uint8_t g, uint8_t b) __attribute__((const));
inline static
uint16_t rgb(uint8_t r, uint8_t g, uint8_t b) {return ((uint16_t)r<<8)|(g<<4)|b;}
uint16_t mixColor(uint16_t color1, uint16_t color2, uint8_t w1, uint8_t w2);
uint16_t hsv(int16_t h, uint8_t s, uint8_t v);
uint16_t hsvx(uint8_t h, uint8_t s, uint8_t v);
uint16_t randomNumber(uint16_t lo, uint16_t hi);
uint16_t randomBits(uint8_t zeros, uint8_t ones);
uint32_t random32();
static inline
uint8_t random8() {
return random32() & 0xff;
}
static inline
uint16_t random16() {
return random32() & 0xffff;
}
#endif
all: libbob3.a
##################
# Compiler setup
PREFIX = /opt/avr/avr-gcc-4.3.4/bin/
GCC_VERSION=-4.3.4
# PREFIX =
# GCC_VERSION=
##################
# AVR compiler setup
CC = $(PREFIX)avr-gcc$(GCC_VERSION)
OBJCOPY = $(PREFIX)avr-objcopy
OBJDUMP = $(PREFIX)avr-objdump
SIZE = $(PREFIX)avr-size
AR = $(PREFIX)avr-ar
##################
# Platforms
LIBS =
##################
# Math and printf
MATH =
PRINTF =
##################
# Platform
DEVICE = atmega88
F_CPU = 8000000
PLATFORM = B-O-B-3
LIBFOLDER = m88-8
EFUSE=07
HFUSE=df
LFUSE=e2
##################
# NIBObee Lib
CFLAGS += -D_BOB3_
CCFLAGS += -D_BOB3_
PROGRAMMER = avrispv2
CFLAGS += -std=c99 -Os -ffunction-sections -DAVR -Wall -I..
CCFLAGS += -Os -ffunction-sections -DAVR -Wall -I..
##################
# Standard Flags
CFLAGS += -mmcu=$(DEVICE) -DF_CPU=$(F_CPU)
CCFLAGS += -mmcu=$(DEVICE) -DF_CPU=$(F_CPU)
CLDFLAGS += -mmcu=$(DEVICE)
##################
# General Rules
LINECOUNT_CMD = 'a=`cat \$0|wc -l` ; echo "compiled $a lines" >>compiler.err'
%.o: %.c
$(CC) $(CFLAGS) -c $< -o $@
@$(SIZE) -B $@ >>sizes.log
%.o: %.cpp
$(CC) $(CCFLAGS) -c $< -o $@
@$(SIZE) -B $@ >>sizes.log
libbob3.a: analog.o bob3.o ircom.o leds.o main.o
%.a:
rm -f $@
$(AR) -q $@ $^
clean:
rm -f *.d *.o *~ *.elf *.log *.err $(BASENAME).hex $(TEMP_FILES) $(BASENAME).xhex $(BASENAME).lss
.PHONY: all clean clean-errors %.c-compile link-success compile-success compile-info link-info build-success
.SECONDARY: $(BASENAME).hex $(BASENAME).xhex $(BASENAME).elf $(BASENAME).o $(OBJS) $(LOBJS)
This diff is collapsed.
/* BSD-License:
Copyright (c) 2016 by Nils Springob, nicai-systems, Germany
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name nicai-systems nor the names of its contributors may be
used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*! @file analog.h
* @brief Die Funktionen ermöglichen den Zugriff auf die analogen Eingänge (Sensorbricks, KEY und Versorgungsspannung).
*
* Die analogen Eingänge werden automatisch im Interrupt gemessen.
* Damit die anlogen Daten gemessen werden können, muss zuerst (zum Beispiel in der setup() Funktion) die Funktion analog_init() aufgerufen werden.
* Falls nicht die Processing-Umgebung <niboburger/robomain.h> verwendet wird, müssen auch die globalen Interrupts manuell eingeschaltet werden.
*
* <b>Anhaltswerte:</b> Die IR-Sensoren liefern für nahe Objekte Werte >50 und für Freiräume Werte <10.
*/
#ifndef _BOB3_ANALOG_H_
#define _BOB3_ANALOG_H_
#ifdef __cplusplus
extern "C" {
#endif
/*!
* Symbolische Namen für den Parameter idx der Funktionen
* analog_getValue(), analog_getValueExt() und analog_setExtToggleMode()
*/
enum {
ANALOG_L0,
ANALOG_L1,
ANALOG_L2,
ANALOG_L3,
ANALOG_R0,
ANALOG_R1,
ANALOG_R2,
ANALOG_R3,
ANALOG_IR,
ANALOG_VOLT, //!< Versorgungsspannung
ANALOG_TEMP,
ANALOG_CNT //!< Anzahl der enums, if(idx < ANALOG_CNT) {/* Index OK */}
};
/*!
* Symbolische Namen für den Parameter mode der
* analog_getValueExt() - Funktionen
*/
enum {
ANALOG_PASSIVE = 0, //!< = 0: passive Messung (LED aus)
ANALOG_ACTIVE = 1, //!< = 1: aktive Messung (LED an)
ANALOG_DIFFERENTIAL = 2 //!< = 2: aktive - passive Messung (Differenz)
};
/*!
* Initialisierung des Liniensensors.
* Der AD-Wandler misst im Interruptmodus automatisch alle Werte, und schaltet
* auch die IR-LEDs der Bodensensoren bei Bedarf ein und aus, wenn mit analog_setExtToggleMode()
* der Togglemode für den jeweiligen Eingang aktiviert wurde.
* @note Die Interrupts müssen aktiviert sein, damit die Sensorwerte gemessen werden!
*/
void analog_init();
/*!
* Analoge Messungen deaktivieren. Durch einen Aufruf der Funktion analog_reenable() werden die Messungen wieder aktiviert.
*/
uint8_t analog_enable(uint8_t enable);
/*!
* Liefert den Wert eines analogen Eingangs mit dem index @param idx zurück.
* @note Die Interrupts müssen aktiviert sein, damit die Werte gemessen werden!
*/
uint16_t analog_getValue(uint8_t idx);
/*!
* Liefert den Wert eines analogen Eingangs mit dem index @param idx zurück.
* Der Parameter @param mode bestimmt die Sensor Messwerte Quelle:
* @ref ANALOG_PASSIVE = bei abgeschalteter Sensor LED,
* @ref ANALOG_ACTIVE = bei eingeschalteter Sensor LED,
* @ref ANALOG_DIFFERENTIAL = Differenzwert ANALOG_ACTIVE - ANALOG_PASSIVE.
* Falls die Differenz negativ wäre, wird eine Null zurückgeliefert!
* Die Werte für ANALOG_ACTIVE und ANALOG_DIFFERENTIAL gelten nur für die Sensorbricks
* und sind nur gültig, wenn der Toggle-Modus aktiviert ist!
* @return Value ADC Wert 0..1024
*
* @code
#include <niboburger/robomain.h>
#include <niboburger/analog.h>
// Minimaler Reflektionswert bei Bodenkontakt:
#define BC_MINVAL 40
void setup() {
// Toggle-Mode für mittleren Bodensensor aktivieren:
analog_setExtToggleMode(ANALOG_BC, ON);
// ...
}
void loop() {
if (analog_has_update()) {
// wenn neue Messwerte vorhanden sind
// Differenz Mittlerer Bodensensor bei LED aus / LED an -> reflektierter Anteil
uint16_t bc = analog_getValueExt(ANALOG_BC, ANALOG_DIFFERENTIAL);
if (bc < BC_MINVAL) {
// Aktion bei Absturzgefahr
engine_stop();
}
// ...
}
}
@endcode
* @note Die Interrupts müssen aktiviert sein, damit die Werte gemessen werden!
*/
uint16_t analog_getValueExt(uint8_t idx, uint8_t mode);
/*!
* Liefert einen Zufallswert zurück. Der Wert errechnet sich aus der Summe
* aller gemessenen Analog-Spannungen. Der Wert selbst sollte nur als Seed
* (Entropiequelle) verwendet werden, da er monoton wachsend ist. Der Wert
* wird um so zufälliger, je mehr analoge Messungen vorher durchgeführt
* wurden.
* @return Random Seed (16 Bit Zufallswert)
*
* @code
#include <niboburger/robomain.h>
#include <niboburger/analog.h>
#include <stdlib.h>
void setup() {
analog_init();
// kurz abwarten, damit einige ADC Messungen durchgeführt werden können:
delay(100);
// Startwert (seed) für den Zufallszahlengenerator (Funktion rand()) auf den echt
// zufälligen Wert (analog_getRandomSeed()) setzen setzen:
srand(analog_getRandomSeed());
}
void loop() {
// Neue Zufallszahl mit dem Generator erzeugen:
int zufallszahl = rand();
// ...
}
@endcode
* @note Die Interrupts müssen aktiviert sein, damit die Werte gemessen werden!
*/
uint16_t analog_getRandomSeed();
/*!
* Aktiviert den automatischen Toggle-Mode (active/passive). Dabei wird die automatisch immer wieder ein und
* ausgeschaltet. Die Funktion analog_getValueExt() liefert dabei in Abhängigkeit vom Parameter mode
* die Werte bei eingeschalteter LED, bei ausgeschalteter LED oder auch die Differenz der beiden Werte zurück.
* @ref ANALOG_BCL und @ref ANALOG_BCR müssens extra aktiviert werden, alle anderen sind nach dem Start bereits automatisch
* im Toggle-Modus.
*/
void analog_setExtToggleMode(uint8_t idx, uint8_t activate);
/*!
* Diese Funktion wird nach einem kompletten Durchgang aller Sampling-Kanäle.
* Als 'weak-linking' Funktion kann sie einfach durch eine eigene Funktion void analog_irq_hook() {}
* ersetzt werden.
* @note Achtung: Funktion wird aus dem IRQ Kontext aufgerufen!
*/
void analog_irq_hook();
/*!
* Diese Funktion wartet bis ein neuer Satz analoger Werte gemessen wurde.
*/
void analog_wait_update();
/*!
* Diese Funktion liefert TRUE wenn seit dem letzten Aufruf ein neuer Satz analoger Werte gemessen wurde.
*/
uint8_t analog_has_update();
#ifndef DOXYGEN
extern uint16_t analog_samples[];
#define analog_getValue_ISR(idx) (analog_samples[((idx))])
#define analog_getValueAct_ISR(idx) (analog_samples[((idx))+ANALOG_CNT])
#endif
#define ADC_BANDGAP_CHANNEL_VOLTAGE 1.1
#ifdef __cplusplus
} // extern "C"
#endif
#endif // _NIBOBURGER_ANALOG_H_
/* BSD-License:
Copyright (c) 2015 by Nils Springob, nicai-systems, Germany
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation