Commit 3ddfe3a0 authored by springob's avatar springob
Browse files

* 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)
/* 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.c
* @brief Zuordnung der physikalischen Pins zu symbolischen Namen
* @author Nils Springob (nils.springob@nicai-systems.com)
* @date 2015-02-25
*/
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/atomic.h>
#include "iodefs.h"
#include "analog.h"
#ifdef __cplusplus
extern "C" {
#endif
extern uint8_t _bob3_revision;
uint16_t analog_samples[2 * ANALOG_CNT];
uint16_t analog_random_seed;
uint8_t analog_pos;
uint8_t analog_flags;
volatile uint8_t analog_sample_id;
uint16_t analog_sum;
#define AREF_1V1(x) (_BV(REFS1)|_BV(REFS0)|((x)))
#define AREF_AVCC(x) (_BV(REFS0)|((x)))
#define AREF_EXT(x) ((x))
#define AMUX_TEMP 0x08
#define AMUX_1V1 0x0e
#define AMUX_0V 0x0f
void analog_init() {
#if defined(DIDR0)
// DIDR0 = 0xff;
#endif
ADCSRA = _BV(ADPS2) // prescale faktor = 64 ADC laeuft
//| _BV(ADPS1) // mit 8 MHz / 64 = 125 kHz
| _BV(ADPS0) // mit 8 MHz / 32 = 250 kHz
| _BV(ADEN) // ADC an
// | _BV(ADATE) // auto trigger
| _BV(ADIE) // enable interrupt
| _BV(ADSC); // Beginne mit der Konvertierung
DDRC |= _BV(2) | _BV(3) | _BV(4) | _BV(5);
DDRB |= _BV(0);
PORTC |= _BV(0) | _BV(1); // PullUp!
activate_output_bit(IO_ARM_1R);
activate_output_bit(IO_ARM_1C);
activate_output_bit(IO_ARM_2R);
activate_output_bit(IO_ARM_2C);
activate_output_bit(IO_EN_IR);
if (_bob3_revision==103) {
deactivate_output_bit(IO_HEAD_S);
}
if (SREG&_BV(7)) {
// measure all once!
analog_wait_update();
} else {
// precharge Key and Voltage input
analog_samples[ANALOG_VOLT] = analog_samples[ANALOG_VOLT+ANALOG_CNT] = (ADC_BANDGAP_CHANNEL_VOLTAGE*1024)/5;
}
}
uint8_t analog_enable(uint8_t enable) {
uint8_t result = (ADCSRA & _BV(ADIE))?0xff:0x00;
if (enable) {
ADCSRA |= _BV(ADEN);
ADCSRA |= _BV(ADIE);
} else {
ADCSRA &= ~_BV(ADIE);
ADCSRA &= ~_BV(ADEN);
}
return result;
}
uint16_t analog_getValue(uint8_t idx) {
//ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
uint16_t retval = 0;
{