* 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

BOB3.cpp

+#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;
+}

BOB3.h

+#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

bob3/Makefile

+
+
+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)

bob3/analog.c

+/*  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;
+  {
+    uint8_t bak = SREG;
+    cli();
+    retval = analog_samples[idx];