Revision 0:e0131e671ea0, committed 2014-04-25

Comitter:

hollegha2

Date:

Fri Apr 25 14:35:21 2014 +0000

Child:

1:cb66be3bb78f

Child:

2:16a5d3302ddb

Commit message:

Initial Rev.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Bertl14.cpp	Fri Apr 25 14:35:21 2014 +0000
@@ -0,0 +1,147 @@
+
+#include "mbed.h"
+#include "Bertl14.h"
+
+Motor::Motor(PinName pwm, PinName fwd, PinName rev) :
+_pwm(pwm), _fwd(fwd), _rev(rev) 
+{
+	_pwm.period(0.001); _pwm=0;
+	_fwd=0; _rev=0;
+}
+
+void Motor::SetPow(float aPow)
+{
+	if( aPow>=0.0 ) {
+		_fwd=1; _rev=0;
+		_pwm = aPow;
+	}
+	else {
+		_fwd=0; _rev=1;
+		_pwm = -aPow;
+	}
+}
+
+BertlDrive::BertlDrive(PinName pwm, PinName fwd, PinName rev, PinName encoder) :
+Motor(pwm,fwd,rev) , _enc(encoder)
+{
+	encCnt = 0;
+}
+
+void BertlDrive::Init()
+{
+	_enc.rise(this, &BertlDrive::EncoderISR);
+	_enc.fall(this, &BertlDrive::EncoderISR);
+}
+
+void BertlDrive::EncoderISR()
+{
+	encCnt++;
+}
+
+
+PortEx::PortEx() : 
+_i2c(p28,p27), _p6Event(p6)
+{
+	btns=btnEvent=0;
+	useISR = 1;
+}
+
+void PortEx::Init()
+{
+	char cmd[4];
+	_i2c.frequency(100000);
+	wait(0.01);
+  // Port0 Config  Port0 Out    Port1 In
+	cmd[0]=0x06;     cmd[1]=0x00; cmd[2]=0xFF;
+	_i2c.write(DEV, cmd, 3, false);
+	SetLedPort(0);
+	_p6Event.fall(this, &PortEx::p6ISR);
+}
+
+void PortEx::p6ISR()
+{
+	if( !useISR )
+		return;
+	int16_t prev = btns;
+  ReadButtons();
+	if( !btns )
+		btns = prev;
+	else
+		btnEvent = 1;
+}
+
+void PortEx::SetLedPort(uint8_t aBitPattern)
+{
+	char cmd[4];
+	cmd[0]=2; cmd[1]=~aBitPattern;
+	_i2c.write(DEV, cmd, 2, false);
+}
+
+void PortEx::SetLeds(uint8_t aBitPattern)
+{
+	_currLeds |= aBitPattern;
+	SetLedPort(_currLeds);
+}
+
+void PortEx::ToggleLeds(uint8_t aBitPattern)
+{
+	_currLeds ^= aBitPattern;
+	SetLedPort(_currLeds);
+}
+
+void PortEx::ClearLeds()
+{
+	_currLeds=0; SetLedPort(0);
+}
+
+
+void PortEx::ReadButtons()
+{
+	char cmd[4];
+	cmd[0]=1;
+	_i2c.write(DEV, cmd, 1, true);
+	_i2c.read(DEV|1, cmd, 1, false);
+	btns = cmd[0];
+}
+
+void PortEx::WaitUntilButtonPressed()
+{
+	int prev = useISR;
+	useISR = 0;
+	btns = 0;
+	while(1) {
+		ReadButtons();
+		if( btns )
+			break;
+		wait(0.01);
+	}
+	btns=btnEvent=0;
+	useISR = prev;
+}
+
+
+
+UsDistSens::UsDistSens(PinName pinTrigger, PinName pinEcho) :
+trigger(pinTrigger),echo(pinEcho)
+{
+	echo.rise(this, &UsDistSens::RisingISR);
+	echo.fall(this, &UsDistSens::FallingISR);
+}
+
+void UsDistSens::StartMeas()
+{
+	trigger=1; wait_us(12); trigger=0;
+	stw.start();
+}
+
+void UsDistSens::RisingISR()
+{ stw.reset(); }
+
+void UsDistSens::FallingISR()
+{
+	dist=stw.read_us();
+	distCM = (float)dist*(343.2E-4/2.0);
+}
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Bertl14.h	Fri Apr 25 14:35:21 2014 +0000
@@ -0,0 +1,100 @@
+
+#ifndef Bertl14_h
+#define Bertl14_h
+
+class Motor {
+	public:
+		Motor(PinName pwm, PinName fwd, PinName rev);
+		void SetPow(float aPow);
+	protected:
+    PwmOut _pwm;
+    DigitalOut _fwd;
+    DigitalOut _rev;
+};
+
+class BertlDrive : public Motor {
+	public:
+		int16_t encCnt;
+	public:
+		BertlDrive(PinName pwm, PinName fwd, PinName rev, PinName encoder);
+		void Init();
+	private:
+		void EncoderISR();
+		InterruptIn _enc;
+};
+
+
+const int BTN_FLL = 0x80;
+const int BTN_FL  = 0x04;
+const int BTN_FM  = 0x01;
+const int BTN_FR  = 0x08;
+const int BTN_FRR = 0x40;
+const int BTN_BL = 0x10;
+const int BTN_BM = 0x02;
+const int BTN_BR = 0x20;
+
+const int LED_FL1 = 0x01; // white die vordere
+const int LED_FL2 = 0x02; // red die hintere
+const int LED_FR1 = 0x04; // white
+const int LED_FR2 = 0x08; // red
+const int LED_ALL_FRONT = 0x0F;
+
+const int LED_BL1 = 0x20; // red back left outher
+const int LED_BL2 = 0x10; // red back left inner
+const int LED_BR1 = 0x80; // red back right outher
+const int LED_BR2 = 0x40; // red back right inner
+const int LED_ALL_BACK = 0xF0;
+
+
+class PortEx {
+	public:
+		// Current State of Buttons is refreshed with ReadButtons()
+		int16_t btns;
+		uint8_t btnEvent;
+		uint8_t useISR;
+	public:
+		PortEx();
+		void Init();
+		
+		void SetLedPort(uint8_t aBitPattern); // NO local Bit-OR
+		void SetLeds(uint8_t aBitPattern);
+		void ToggleLeds(uint8_t aBitPattern);
+		void ClearLeds();
+		
+		void ReadButtons();
+		void WaitUntilButtonPressed();
+
+		bool IsButton(int aBitPattern)
+			{ return btns & aBitPattern; }
+		
+		bool IsAnyFrontButton()
+			{ return btns & (BTN_FL|BTN_FM|BTN_FR); }
+		
+		bool IsAnyBackButton()
+			{ return btns & (BTN_BL|BTN_BM|BTN_BR); }
+	private:
+		uint8_t _currLeds;
+		void p6ISR();
+		I2C _i2c;
+		const int DEV = 0x40;
+		InterruptIn _p6Event;
+};
+
+
+class UsDistSens {
+	public:
+		UsDistSens(PinName pinTrigger, PinName pinEcho);
+		void StartMeas();
+	private:
+		void RisingISR();
+		void FallingISR();
+	private:
+		DigitalOut trigger;
+		InterruptIn echo;
+		Timer stw;
+	public:
+		int dist;
+		float distCM;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BertlObjects.h	Fri Apr 25 14:35:21 2014 +0000
@@ -0,0 +1,20 @@
+
+
+
+BusOut leds(LED1,LED2,LED3,LED4);
+// DigitalOut ledBlue(P1_28); // 3 blaue LEDs
+
+BertlDrive mL(p34, P1_1, P1_0, P1_12); 
+BertlDrive mR(p36, P1_4, P1_3, P1_13); 
+
+PortEx pex;
+UsDistSens us(p21,p22);
+
+void InitBertl()
+{
+	leds=0;
+  mL.Init(); mR.Init(); pex.Init();
+	pex.useISR=0;
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Serial_HL.cpp	Fri Apr 25 14:35:21 2014 +0000
@@ -0,0 +1,130 @@
+
+#include "Serial_HL.h"
+#include <stdarg.h>
+#include <stdio.h>
+
+namespace mbed {
+
+SerialBLK::SerialBLK(PinName tx, PinName rx) {
+  serial_init(&_serial, tx, rx);
+  _baud = 9600;
+  serial_irq_handler(&_serial, SerialBLK::_irq_handler, (uint32_t)this);
+}
+
+void SerialBLK::baud(int baudrate) {
+  serial_baud(&_serial, baudrate);
+  _baud = baudrate;
+}
+
+void SerialBLK::format(int bits, Parity parity, int stop_bits) {
+  serial_format(&_serial, bits, (SerialParity)parity, stop_bits);
+}
+
+int SerialBLK::readable() {
+  return serial_readable(&_serial);
+}
+
+int SerialBLK::writeable() {
+  return serial_writable(&_serial);
+}
+
+void SerialBLK::attach(void (*fptr)(void), IrqType type) {
+  if (fptr) {
+      _irq[type].attach(fptr);
+      serial_irq_set(&_serial, (SerialIrq)type, 1);
+  } else {
+      serial_irq_set(&_serial, (SerialIrq)type, 0);
+	}
+}
+
+void SerialBLK::_irq_handler(uint32_t id, SerialIrq irq_type) {
+  SerialBLK *obj = (SerialBLK*)id;
+  obj->_irq[irq_type].call();
+}
+
+int SerialBLK::GetChar() {
+  return serial_getc(&_serial);
+}
+
+void SerialBLK::PutChar(int aCh) {
+  serial_putc(&_serial, aCh);
+}
+
+void SerialBLK::Write(void* aData, uint32_t aLenBytes)
+{
+	uint8_t* ptr = (uint8_t*)aData;
+	for(int i=0; i<aLenBytes; i++) {
+		this->PutChar(*ptr); ptr++;
+	}
+}
+
+void SerialBLK::Read(void* aData, uint32_t aLenBytes)
+{
+	uint8_t* ptr = (uint8_t*)aData;
+  for(int i=0; i<aLenBytes; i++) {
+    *ptr=this->GetChar(); ptr++;
+  }
+}
+
+
+
+
+void SvProtocol::Puts(char* aCStr)
+{
+  while( *aCStr != '\0' )
+  {
+    if( *aCStr=='\n' )
+      _st->PutChar('\r');
+    _st->PutChar(*aCStr);
+    aCStr++;
+  }
+	_st->PutChar(0); // terminate with 0
+}
+
+static char sBuffer[50];
+
+void SvProtocol::Printf(const char *format, ...)
+{
+  va_list vArgs;
+  va_start(vArgs, format);
+  vsprintf(sBuffer, (char const *)format, vArgs);
+  va_end(vArgs);
+  Puts(sBuffer);
+}
+
+void SvProtocol::SvPrintf(const char *format, ...)
+{
+  va_list vArgs;
+  va_start(vArgs, format);
+  vsprintf(sBuffer, (char const *)format, vArgs);
+  va_end(vArgs);
+  if( !svMessageON ) return;
+  _st->PutChar(10);
+  Puts(sBuffer);
+}
+
+void SvProtocol::WriteSV3p13(int aId, float aData) {
+  // int16_t val = To3p13(aData);
+  // PutChar(aId); Write(&val,2);
+}
+
+int SvProtocol::GetCommand()
+{
+  uint8_t cmd = _st->GetChar();
+  if( cmd==1 )
+  {
+    this->acqON = _st->GetChar();
+    if( this->acqON )
+      this->SvMessage("AcqON");
+    else
+      this->SvMessage("AcqOFF");
+    return 0;
+  }
+  return cmd;
+}
+
+} // namespace mbed
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Serial_HL.h	Fri Apr 25 14:35:21 2014 +0000
@@ -0,0 +1,66 @@
+
+#ifndef Serial_HL_h
+#define Serial_HL_h
+
+#include "platform.h"
+// #include "Stream.h"
+#include "FunctionPointer.h"
+#include "serial_api.h"
+#include "SvProtocol.h"
+
+namespace mbed {
+
+class SerialBLK : public IStreamHL {
+	public:
+		SerialBLK(PinName tx, PinName rx);
+		void baud(int baudrate);
+  
+    virtual void PutChar(int aCh);
+		virtual int GetChar();
+		virtual void Write(void* aData, uint32_t aLenBytes);
+		virtual void Read(void* aData, uint32_t aLenBytes);
+		
+		enum Parity {
+        None = 0,
+        Odd,
+        Even,
+        Forced1,
+        Forced0
+		};
+		enum IrqType {
+        RxIrq = 0,
+        TxIrq
+		};
+    
+    void format(int bits=8, Parity parity=SerialBLK::None, int stop_bits=1);
+		
+		int readable();
+		int writeable();
+		int IsDataAvail()
+			{ return readable(); }
+		
+		// fptr A pointer to a void function, or 0 to set as none
+    // type Which serial interrupt to attach the member function to (Seriall::RxIrq for receive, TxIrq for transmit buffer empty)
+    void attach(void (*fptr)(void), IrqType type=RxIrq);
+		
+		// tptr pointer to the object to call the member function on
+    // mptr pointer to the member function to be called
+    // type Which serial interrupt to attach the member function to (Seriall::RxIrq for receive, TxIrq for transmit buffer empty)
+		template<typename T>
+    void attach(T* tptr, void (T::*mptr)(void), IrqType type=RxIrq) {
+        if((mptr != NULL) && (tptr != NULL)) {
+            _irq[type].attach(tptr, mptr);
+            serial_irq_set(&_serial, (SerialIrq)type, 1);
+        }
+    }
+		
+		static void _irq_handler(uint32_t id, SerialIrq irq_type);
+	protected:
+		serial_t        _serial;
+    FunctionPointer _irq[2];
+    int             _baud;
+};
+
+} // namespace mbed
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SvProtocol.h	Fri Apr 25 14:35:21 2014 +0000
@@ -0,0 +1,83 @@
+
+#ifndef SvProtocol_h
+#define SvProtocol_h
+
+#include "stdint.h"
+
+namespace mbed {
+
+class IStreamHL {
+	public:
+		virtual void PutChar(int aCh) = 0;
+		virtual int GetChar() = 0;
+		virtual void Write(void* aData, uint32_t aLenBytes) = 0;
+		virtual void Read(void* aData, uint32_t aLenBytes) = 0;
+};
+
+class SvProtocol {
+	public:
+		IStreamHL* _st;
+		uint8_t acqON;
+    uint8_t svMessageON;
+	public:
+		SvProtocol(IStreamHL* aStrm) {
+			acqON=0; svMessageON=1;
+			_st=aStrm; 
+		}
+	
+		// Check's first for acqOn/Off Command
+    // ret 0 if acqOn/Off was handled in GetCommand
+    int GetCommand();
+		
+		void Puts(char* aCStr); // Terminate with 0
+
+    // \r\n is appended automatically
+    void Printf(const char* format, ...);
+
+    void SvPrintf(const char *format, ...);
+    
+    void WriteSV(int aId, char* aData) {
+      if( !svMessageON ) return;
+      _st->PutChar(aId); Puts(aData); 
+    }
+    
+    void SvMessage(char* aTxt) {
+      if( !svMessageON ) return;
+      _st->PutChar(10); Puts(aTxt);
+    }
+    
+    void VectHeader(int aId, int aNVals)
+      { _st->PutChar(aId); _st->PutChar(aNVals); }
+    
+    void WriteSvI16(int aId, int16_t aData)
+      { _st->PutChar(aId+10); _st->Write(&aData,2); }
+    
+    void WriteSvI32(int aId, int32_t aData)
+      { _st->PutChar(aId); _st->Write(&aData,4); }
+    
+    void WriteSV(int aId, float aData)
+      { _st->PutChar(aId+20); _st->Write(&aData,4); }
+    
+    // float in 3.13 Format
+    void WriteSV3p13(int aId, float aData);
+
+    int16_t ReadI16()
+      { int16_t ret; _st->Read(&ret,2); return ret; }
+
+    int32_t ReadI32()
+      { int32_t ret; _st->Read(&ret,4); return ret; }
+    
+    float ReadF()
+      { float ret; _st->Read(&ret,4); return ret; }
+};
+
+} // namespace mbed
+
+// SV-Id Ranges and DataTypes for SvVis3 Visualisation-Tool
+//----------------------------------------------------------
+// Id = 10       : string
+// Id = 1 .. 9   : format 3.13  2 Bytes
+// Id = 11 .. 20 : short        2 Bytes
+// Id = 21 .. 30 : float        4 Bytes
+
+#endif