Christian Burri
This program is for an autonomous robot for the competition at the Hochschule Luzern. We are one of the 32 teams. <a href="http://cruisingcrepe.wordpress.com/">http://cruisingcrepe.wordpress.com/</a> The postition control is based on this Documentation: Control of Wheeled Mobile Robots: An Experimental Overview from Alessandro De Luca, Giuseppe Oriolo, Marilena Vendittelli. For more information see here: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>
Fork of
autonomousRobotAndroid
by 
Christian Burri
Revision 0:31f7be68e52d, committed 2013-02-07
- Comitter:
- chrigelburri
- Date:
- Thu Feb 07 17:43:19 2013 +0000
- Child:
- 1:6cd533a712c6
- Commit message:
- first steps
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Actuators/Hallsensor.lib Thu Feb 07 17:43:19 2013 +0000 @@ -0,0 +1,1 @@ +Actuators/Hallsensor#d5573b3c22ae
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuators/Hallsensor/Hallsensor.cpp Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,115 @@
+#include "Hallsensor.h"
+
+Hallsensor::Hallsensor(
+ PinName hall1,
+ PinName hall2,
+ PinName hall3
+)
+ : hall1_(hall1),
+ hall2_(hall2),
+ hall3_(hall3)
+{
+ pulses_ = 0;
+
+ //Workout what the current state is.
+ int h1 = hall1_.read();
+ int h2 = hall2_.read();
+ int h3 = hall3_.read();
+
+ // Set the PullUp Resistor
+ hall1_.mode(PullUp);
+ hall2_.mode(PullUp);
+ hall3_.mode(PullUp);
+
+ //set interrupts on only hall 1-3 rise and fall.
+ hall1_.rise(this, &Hallsensor::encode);
+ hall1_.fall(this, &Hallsensor::encode);
+ hall2_.rise(this, &Hallsensor::encode);
+ hall2_.fall(this, &Hallsensor::encode);
+ hall3_.rise(this, &Hallsensor::encode);
+ hall3_.fall(this, &Hallsensor::encode);
+}
+
+void Hallsensor::reset(void)
+{
+ pulses_ = 0;
+}
+
+int Hallsensor::getPulses(void)
+{
+ return pulses_;
+}
+
+
+void Hallsensor::encode(void)
+{
+ // read the state
+ int h1 = hall1_.read();
+ int h2 = hall2_.read();
+ int h3 = hall3_.read();
+
+ //3-bit state.
+ currState_ = (h1 << 2) | (h2 << 1) | h3; // | Bitweise oder Verknüpfung Beispiel: 1100 | 1010 = 1110
+
+ if ((prevState_ == 0x5) && (currState_ == 0x4)) {
+ pulses_++;
+ prevState_ = currState_;
+ return;
+ } else if (prevState_ == 0x5 && currState_ == 0x1) {
+ pulses_--;
+ prevState_ = currState_;
+ return;
+ }
+
+ if ((prevState_ == 0x4) && (currState_ == 0x6)) {
+ pulses_++;
+ prevState_ = currState_;
+ return;
+ } else if (prevState_ == 0x4 && currState_ == 0x5) {
+ pulses_--;
+ prevState_ = currState_;
+ return;
+ }
+
+ if ((prevState_ == 0x6) && (currState_ == 0x2)) {
+ pulses_++;
+ prevState_ = currState_;
+ return;
+ } else if (prevState_ == 0x6 && currState_ == 0x4) {
+ pulses_--;
+ prevState_ = currState_;
+ return;
+ }
+
+ if ((prevState_ == 0x2) && (currState_ == 0x3)) {
+ pulses_++;
+ prevState_ = currState_;
+ return;
+ } else if (prevState_ == 0x2 && currState_ == 0x6) {
+ pulses_--;
+ prevState_ = currState_;
+ return;
+ }
+
+ if ((prevState_ == 0x3) && (currState_ == 0x1)) {
+ pulses_++;
+ prevState_ = currState_;
+ return;
+ } else if (prevState_ == 0x3 && currState_ == 0x2) {
+ pulses_--;
+ prevState_ = currState_;
+ return;
+ }
+
+ if ((prevState_ == 0x1) && (currState_ == 0x5)) {
+ pulses_++;
+ prevState_ = currState_;
+ return;
+ } else if (prevState_ == 0x1 && currState_ == 0x3) {
+ pulses_--;
+ prevState_ = currState_;
+ return;
+ }
+ prevState_ = currState_;
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuators/Hallsensor/Hallsensor.h Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,84 @@
+#ifndef HALLSENSOR_H
+#define HALLSENSOR_H
+
+#include "mbed.h"
+
+/**
+ * @author Christian Burri
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
+ * All rights reserved.
+ *
+ * @section DESCRIPTION
+ *
+ * Interface to count the Hallsensor input from a EC-Motor.
+ *
+ */
+class Hallsensor
+{
+
+public:
+
+ /**
+ * Constructor.
+ *
+ * Reads the current values on Hall1 , Hall2 and Hall3 to determine the
+ * initial state.
+ *
+ * Attaches the encode function to the rise/fall interrupt edges of
+ * Hall1, Hall2 and Hall3.
+ *
+ *
+ * @param hall1 mbed pin for Hall1 input.
+ * @param hall2 mbed pin for Hall2 input.
+ * @param hall3 mbed pin for Hall3 input.
+ */
+ Hallsensor(PinName hall1, PinName hall2, PinName hall3);
+
+ /**
+ * Reset the encoder.
+ *
+ * Sets the pulses and revolutions count to zero.
+ */
+ void reset(void);
+
+ /**
+ * Read the number of pulses recorded by the encoder.
+ *
+ * @return Number of pulses which have occured.
+ */
+ int getPulses(void);
+
+ /**
+ * Read the number of revolutions recorded by the encoder on the index channel.
+ *
+ * @return Number of revolutions which have occured on the index channel.
+ */
+ int getRevolutions(void);
+
+private:
+
+ /**
+ * Update the pulse count.
+ *
+ * Called on every rising/falling edge of Hall 1-3.
+ *
+ * Reads the state of the channels and determines whether a pulse forward
+ * or backward has occured, updating the count appropriately.
+ */
+ void encode(void);
+
+ InterruptIn hall1_;
+ InterruptIn hall2_;
+ InterruptIn hall3_;
+
+ int prevState_;
+ int currState_;
+
+ volatile int pulses_;
+
+};
+
+#endif /* Hallsensor_H */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Actuators/MaxonESCON.lib Thu Feb 07 17:43:19 2013 +0000 @@ -0,0 +1,1 @@ +Actuators/MaxonESCON#343a651a9693
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuators/MaxonESCON/MaxonESCON.cpp Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,134 @@
+#include "MaxonESCON.h"
+
+using namespace std;
+
+MaxonESCON::MaxonESCON(
+ PinName enb,
+ PinName isenb,
+ PinName pwm,
+ PinName actualSpeed,
+ Hallsensor *hall
+)
+ :
+ _enb(enb),
+ _isenb(isenb),
+ _pwm(pwm),
+ _actualSpeed(actualSpeed),
+ _hall(hall)
+{
+
+ _pwm = 0;
+
+ // Initial condition of output enables
+ _enb = 0;
+
+ // Set initial condition of PWM 2kHz
+ period(0.0005);
+
+ // Set the pulses to zero
+ _pulses = 0;
+
+ // Set the Pull Up Resistor
+ _isenb.mode(PullUp);
+}
+
+void MaxonESCON::setVelocity(float speed)
+{
+ speed = speed / ESCON_SET_FACTOR * 60.0f;
+ if(speed > 1 ) {
+ _pwm = 0.9f;
+ } else if(speed < -1) {
+ _pwm = 0.1f;
+ } else {
+ _pwm = 0.4f*speed + 0.5f;
+ }
+}
+
+float MaxonESCON::getActualSpeed(void)
+{
+ return (_actualSpeed.read()* 2.0f - 1.0f) * ESCON_GET_FACTOR / 60.0f;
+}
+
+void MaxonESCON::period(float period)
+{
+ _pwm.period(period);
+}
+
+void MaxonESCON::enable(bool enb)
+{
+ if(enb == false) {
+ _enb = 0;
+ } else {
+ _enb = 1;
+ }
+}
+
+bool MaxonESCON::isEnabled()
+{
+ if(_isenb.read() == 1) {
+ return true;
+ } else {
+ return false;
+ }
+ /*
+ if(_enb == 0) {
+ return false;
+ } else {
+ return true;
+ }*/
+}
+
+int MaxonESCON::getPulses(void)
+{
+ _pulses = _hall->getPulses();
+ return _pulses;
+}
+
+int MaxonESCON::setPulses(int setPos)
+{
+ _hall->reset();
+ _pulses = _hall->getPulses();
+ return _pulses;
+}
+
+
+///// Für Polling muss mit unter Code gearbeitet werden.
+
+/*
+int MaxonESCON::getHallPosition(void)
+{
+ return HALL_POSITION[(_hall1.read() ? 1 : 0)+(_hall2.read() ? 2 : 0)+(_hall3.read() ? 4 : 0)];
+}
+
+int MaxonESCON::getPosition(void)
+{
+ position calculation
+
+int actualHallPosition = getHallPosition();
+
+if (actualHallPosition > 0) {
+ if ((actualHallPosition == 1) && (hallPosition >= 5)) {
+ turns++;
+ } else if ((actualHallPosition == 2) && (hallPosition == 6)) {
+ turns++;
+ } else if ((actualHallPosition == 5) && (hallPosition == 1)) {
+ turns--;
+ } else if ((actualHallPosition == 6) && (hallPosition <= 2)) {
+ turns--;
+ }
+ hallPosition = actualHallPosition;
+}
+return turns;
+}
+
+float MaxonESCON::getTransPosition(void)
+{
+return (turns * 2 * _wheelRadius * pi) / (_gear * _polePairs); // PULSES_PER_STEP muss weg da nach 6 hallimpus ein turn gibt
+}
+
+int MaxonESCON::setPosition(int setPos)
+{
+turns = setPos;
+return turns;
+}
+*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuators/MaxonESCON/MaxonESCON.h Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,109 @@
+#ifndef _MAXON_ESCON_H_
+#define _MAXON_ESCON_H_
+
+#include "mbed.h"
+#include "Hallsensor.h"
+#include "defines.h"
+
+/**
+ * @author Christian Burri
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
+ * All rights reserved.
+ *
+ * @section DESCRIPTION
+ *
+ * This class implements the driver for the Maxon ESCON servo driver.....
+ *
+ * Datasheet:
+ *
+ * http://escon.maxonmotor.com
+ */
+class MaxonESCON
+{
+
+protected:
+
+ /** Duty Cycle to set the speed */
+ PwmOut _pwm;
+ /** To Enable the amplifier */
+ DigitalOut _enb;
+ /** Hallsensor Class */
+ Hallsensor* _hall;
+ /** Ready output from ESCON */
+ DigitalIn _isenb;
+ /** Actual speed from ESCON analog Output 1 */
+ AnalogIn _actualSpeed;
+
+private:
+
+ /** increment the Hallpattern */
+ int _pulses;
+
+public:
+
+ /** Create a motor control object
+ *
+ * @param enb DigitalOut, set high for enable
+ * @param isenb DigitalIn, high for enable
+ * @param pwm PwmOut pin, set the Velocity
+ * @param hall1 HALL Object
+ * @param Actual Speed AnalogIn Filtered Signal for ActualSpeed from Motor
+ */
+ MaxonESCON(PinName enb,
+ PinName isenb,
+ PinName pwm,
+ PinName actualSpeed,
+ Hallsensor *hall);
+
+ /** Set the speed of the motor with a pwm for 10%..90%
+ * 50% PWM is 0rpm
+ * Caclulate from 1/s in 1/min plus the Facotr of the ESCON
+ * @param speed The speed of the motor as a normalised value in [1/s]
+ */
+ void setVelocity(float speed);
+
+ /**Return the speed from ESCON
+ *
+ * Analog input 1.65V = 0 rpm
+ *
+ * @return speed of the motor [1/s]
+ */
+ float getActualSpeed(void);
+
+ /** Set the period of the pwm duty cycle.
+ *
+ * Wrapper for PwmOut::period()
+ *
+ * @param seconds - Pwm duty cycle in seconds.
+ */
+ void period(float period);
+
+ /** Set the Motor to a enable sate
+ *
+ * @param enable - 0 for disable 1 for enable.
+ */
+ void enable (bool enb);
+
+ /**Tests if the servo drive is enabled.
+ *
+ * @return <code>true</code> if the drive is enabled, <code>false</code> otherwise.
+ */
+ bool isEnabled(void);
+
+ /** Return the translativ Position
+ *
+ * @return position in meter
+ */
+ int getPulses(void);
+
+ /** Set the Pulses of the Motor
+ *
+ * @return number of turns
+ */
+ int setPulses(int setPos);
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Android/Android.cpp Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,201 @@
+#include "Android.h"
+
+using namespace std;
+
+
+Android::Android(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, float period)
+ :
+ Task(period),
+ AndroidAccessory (OUTL, INBL, MANUFACTURER, MODEL, DESCRIPTION, VERSION, URI, SERIAL)
+{
+ /* get peripherals */
+ this->s = s;
+ this->robotControl = robotControl;
+ this->motorControllerLeft = motorControllerLeft;
+ this->motorControllerRight = motorControllerRight;
+ this->period = period;
+
+ setupDevice();
+ USBInit();
+
+}
+
+
+Android::~Android() {}
+
+
+void Android::run()
+{
+
+ USBLoop();
+
+
+ /*
+
+ SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN
+
+ HIer die Sollwertvorgabe position und Winkel
+ robotContol->setxPosition(1.0f);
+ robotContol->setyPosition(1.0f);
+ robotContol->setTheta(1/2 * piiii);
+
+
+ SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN
+
+
+
+
+ LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN
+
+ Hier die aktuelle Position und Winkel
+ s->xAxis oder robotControl->getxActualPosition();
+ s->yAxis oder robotControl->getyActualPosition();
+ s->angle = robotControl->getActualTheta() * 180 / PI;
+
+
+ Batterie
+ s->voltageBattery
+
+ Status
+ s->state;
+
+ LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN
+
+
+
+
+
+ BEim Start muss das Kommen nach dem Start Klicken muss alles auf null gesetzt werden. und los gehts
+
+ compass.calibrate_compass_basis_rad();
+
+ robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET);
+ leftMotor->setPulses(0);
+ rightMotor->setPulses(0);
+ robotControl->setEnable(false);
+ wait(0.2);
+ robotControl->setEnable(true);
+ wait(0.2);
+ state->startTimerFromZero();
+ state->initPlotFile();
+ state->start();
+
+ ENDE Start
+
+
+
+ Beim ENDE Muss das kommen
+ state->savePlotFile(s);
+ ENDE
+
+ */
+}
+
+int Android::callbackRead(u8 *buf, int len)
+{
+ // pc.printf("%i %s\n\r\n\n\n",len,buf);
+ for (int i = 0; i<INBL; i++) {
+ buf[i] = 0;
+ }
+}
+
+
+void Android::setupDevice()
+{
+ // pc.baud(460800);
+ // pc.printf("Welcome to Android\n\n\n\n\n\n\r");
+ settick = false;
+ // pc.attach(this, &Android::serialIRQ, Serial::RxIrq);
+ for (int i = 0; i<OUTL; i++) {
+ buffer[i] = 0;
+ }
+ bcount = 0;
+ //n.attach(this,&Android::AttachTick,5);
+ //tick.attach(this,&Android::onTick,0.1);
+}
+void Android::resetDevice()
+{
+ //pc.printf("Android reset\n\r");
+ for (int i = 0; i<OUTL; i++) {
+ buffer[i] = 0;
+ }
+ bcount = 0;
+}
+
+int Android::callbackWrite()
+{
+ return 0;
+}
+
+
+void Android::AttachTick()
+{
+ if(!settick)tick.attach(this,&Android::onTick,0.04);
+ settick = true;
+}
+
+void Android::onTick()
+{
+ bool update = false;
+ int templ, tempr;
+
+
+
+ templ = int(left * 10000);
+ tempr = int(right * 10000);
+
+
+
+ if (abs(templ-tl)>170) {
+ update = true;
+ }
+ if (abs(tempr-tr)>170) {
+ update = true;
+ }
+ if (update) {
+ u8* wbuf = _writebuff;
+
+ wbuf[0] = 'P';
+ wbuf[1] = templ&0xFF;
+ wbuf[2] = (templ>>8) & 0xFF;
+ wbuf[3] = tempr&0xFF;
+ wbuf[4] = (tempr>>8) & 0xFF;
+ wbuf[5] = 0;
+
+ this->write(wbuf,5);
+
+ }
+}
+
+
+void Android::serialIRQ()
+{
+ // buffer[bcount] = pc.getc();
+
+ // pc.putc(buffer[bcount]);
+
+ if (buffer[bcount] == '\n' || buffer[bcount] == '\r') {
+ u8* wbuf = _writebuff;
+ for (int i = 0; i<OUTL; i++) {
+ wbuf[i] = buffer[i];
+ buffer[i] = 0;
+ }
+ //pc.printf("Sending: %s\n\r",wbuf);
+ this->write(wbuf,bcount);
+ bcount = 0;
+ } else {
+ if (buffer[bcount] != 0x08 && buffer[bcount] != 0x7F ) {
+ bcount++;
+ if (bcount == OUTL) {
+ bcount = 0;
+ }
+ } else {
+ bcount--;
+ }
+ }
+
+}
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Android/Android.h Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,122 @@
+#ifndef _Android_H_
+#define _Android_H_
+
+#include <cmath>
+#include "mbed.h"
+#include "RobotControl.h"
+#include "AndroidAccessory.h"
+#include "Task.h"
+#include "defines.h"
+
+// Communication
+#define OUTL 100 //The size of the read buffer
+#define INBL 100 //The size of the write buffer
+#define MANUFACTURER "ARM" //The manufacturer of the accessory
+#define MODEL "mbed" //The model of the accessory
+#define DESCRIPTION "mbed Terminal" //A short description of the accessory
+#define VERSION "0.1" //The current version of the accessory
+#define URI "http://www.mbed.org" //Some data to go with the accessory (URL or more description)
+#define SERIAL "0000000012345678" //The serial number of the accessory
+
+
+/**
+ * @author Christian Burri
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
+ * All rights reserved.
+ *
+ * @section DESCRIPTION
+ * IMMPORTANT: OnLoadDevice(..) muss noch bei AndroidAccesory.cpp auskommentiert werden, weil es schon beim BLUE USB vorkommt.
+ * AUCH WICHTIG: Nach dem Löschen von BLUE USB Muss der USBHost neu importiert werden im Andoid/AndroidAccessory Es sind drei Dateinen
+ * Connect to an Android Smartphone......
+ */
+
+class Android : public AndroidAccessory, Task
+{
+
+private:
+
+ state_t* s;
+ RobotControl* robotControl;
+ MaxonESCON* motorControllerLeft;
+ MaxonESCON* motorControllerRight;
+ float period;
+
+public:
+
+
+ /**
+ * Creates a Android object and initializes all private
+ * Android variables.
+ * @param RobotControl Objekt to read the State.
+ * @param motorControllerLeft a reference to the servo drive for the left motor.
+ * @param motorControllerRight a reference to the servo drive for the right motor.
+ * @param period the sampling period of the run loop of this controller, given in [s].
+ */
+ Android(state_t* s, RobotControl* robotControl,MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, float period);
+
+ /**
+ * Destructor of the Object to destroy the Object.
+ **/
+ virtual ~Android();
+
+
+ /** Init the device
+ * This is meant to be implimented by the user of the class
+ *
+ * @param device Device number
+ * @param configuration Configuration
+ * @param interfaceNumber Inteface number
+ */
+ ///////////////////////////// virtual void init (int device, int configuration, int interfaceNumber);
+
+ /** Reset the device
+ * This is meant to be implimented by the user of the class
+ *
+ */
+ virtual void resetDevice ();
+
+ /** Setup the device
+ * This is meant to be implimented by the user of the class. Called when the device is first intialised
+ *
+ */
+ virtual void setupDevice ();
+
+ /** Callback on Read
+ * This is meant to be implimented by the user of the class. Called when some data has been read in.
+ *
+ * @param buff The buffered read in data
+ * @param len The length of the packet recived
+ *
+ */
+ virtual int callbackRead (u8 *buff, int len);
+
+ /** Callback after Write
+ * This is meant to be implimented by the user of the class. Called when the write has been finished.
+ *
+ */
+ virtual int callbackWrite ();
+
+
+ void run();
+
+
+private:
+
+ void serialIRQ();
+ void onTick();
+ void AttachTick();
+ char buffer[INBL];
+ int bcount;
+ Ticker tick;
+ float right,left,rl,ll;
+ int tl,tr;
+ Timeout n;
+ bool settick;
+
+
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Android/AndroidAccessory.lib Thu Feb 07 17:43:19 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/p07gbar/code/AndroidAccessory/#b691d42306d9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PowerControl/EthernetPowerControl.cpp Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,138 @@
+#include "EthernetPowerControl.h"
+
+static void write_PHY (unsigned int PhyReg, unsigned short Value) {
+ /* Write a data 'Value' to PHY register 'PhyReg'. */
+ unsigned int tout;
+ /* Hardware MII Management for LPC176x devices. */
+ LPC_EMAC->MADR = DP83848C_DEF_ADR | PhyReg;
+ LPC_EMAC->MWTD = Value;
+
+ /* Wait utill operation completed */
+ for (tout = 0; tout < MII_WR_TOUT; tout++) {
+ if ((LPC_EMAC->MIND & MIND_BUSY) == 0) {
+ break;
+ }
+ }
+}
+
+static unsigned short read_PHY (unsigned int PhyReg) {
+ /* Read a PHY register 'PhyReg'. */
+ unsigned int tout, val;
+
+ LPC_EMAC->MADR = DP83848C_DEF_ADR | PhyReg;
+ LPC_EMAC->MCMD = MCMD_READ;
+
+ /* Wait until operation completed */
+ for (tout = 0; tout < MII_RD_TOUT; tout++) {
+ if ((LPC_EMAC->MIND & MIND_BUSY) == 0) {
+ break;
+ }
+ }
+ LPC_EMAC->MCMD = 0;
+ val = LPC_EMAC->MRDD;
+
+ return (val);
+}
+
+void EMAC_Init()
+{
+ unsigned int tout,regv;
+ /* Power Up the EMAC controller. */
+ Peripheral_PowerUp(LPC1768_PCONP_PCENET);
+
+ LPC_PINCON->PINSEL2 = 0x50150105;
+ LPC_PINCON->PINSEL3 &= ~0x0000000F;
+ LPC_PINCON->PINSEL3 |= 0x00000005;
+
+ /* Reset all EMAC internal modules. */
+ LPC_EMAC->MAC1 = MAC1_RES_TX | MAC1_RES_MCS_TX | MAC1_RES_RX | MAC1_RES_MCS_RX |
+ MAC1_SIM_RES | MAC1_SOFT_RES;
+ LPC_EMAC->Command = CR_REG_RES | CR_TX_RES | CR_RX_RES;
+
+ /* A short delay after reset. */
+ for (tout = 100; tout; tout--);
+
+ /* Initialize MAC control registers. */
+ LPC_EMAC->MAC1 = MAC1_PASS_ALL;
+ LPC_EMAC->MAC2 = MAC2_CRC_EN | MAC2_PAD_EN;
+ LPC_EMAC->MAXF = ETH_MAX_FLEN;
+ LPC_EMAC->CLRT = CLRT_DEF;
+ LPC_EMAC->IPGR = IPGR_DEF;
+
+ /* Enable Reduced MII interface. */
+ LPC_EMAC->Command = CR_RMII | CR_PASS_RUNT_FRM;
+
+ /* Reset Reduced MII Logic. */
+ LPC_EMAC->SUPP = SUPP_RES_RMII;
+ for (tout = 100; tout; tout--);
+ LPC_EMAC->SUPP = 0;
+
+ /* Put the DP83848C in reset mode */
+ write_PHY (PHY_REG_BMCR, 0x8000);
+
+ /* Wait for hardware reset to end. */
+ for (tout = 0; tout < 0x100000; tout++) {
+ regv = read_PHY (PHY_REG_BMCR);
+ if (!(regv & 0x8000)) {
+ /* Reset complete */
+ break;
+ }
+ }
+}
+
+
+void PHY_PowerDown()
+{
+ if (!Peripheral_GetStatus(LPC1768_PCONP_PCENET))
+ EMAC_Init(); //init EMAC if it is not already init'd
+
+ unsigned int regv;
+ regv = read_PHY(PHY_REG_BMCR);
+ write_PHY(PHY_REG_BMCR, regv | (1 << PHY_REG_BMCR_POWERDOWN));
+ regv = read_PHY(PHY_REG_BMCR);
+
+ //shouldn't need the EMAC now.
+ Peripheral_PowerDown(LPC1768_PCONP_PCENET);
+
+ //and turn off the PHY OSC
+ LPC_GPIO1->FIODIR |= 0x8000000;
+ LPC_GPIO1->FIOCLR = 0x8000000;
+}
+
+void PHY_PowerUp()
+{
+ if (!Peripheral_GetStatus(LPC1768_PCONP_PCENET))
+ EMAC_Init(); //init EMAC if it is not already init'd
+
+ LPC_GPIO1->FIODIR |= 0x8000000;
+ LPC_GPIO1->FIOSET = 0x8000000;
+
+ //wait for osc to be stable
+ wait_ms(200);
+
+ unsigned int regv;
+ regv = read_PHY(PHY_REG_BMCR);
+ write_PHY(PHY_REG_BMCR, regv & ~(1 << PHY_REG_BMCR_POWERDOWN));
+ regv = read_PHY(PHY_REG_BMCR);
+}
+
+void PHY_EnergyDetect_Enable()
+{
+ if (!Peripheral_GetStatus(LPC1768_PCONP_PCENET))
+ EMAC_Init(); //init EMAC if it is not already init'd
+
+ unsigned int regv;
+ regv = read_PHY(PHY_REG_EDCR);
+ write_PHY(PHY_REG_BMCR, regv | (1 << PHY_REG_EDCR_ENABLE));
+ regv = read_PHY(PHY_REG_EDCR);
+}
+
+void PHY_EnergyDetect_Disable()
+{
+ if (!Peripheral_GetStatus(LPC1768_PCONP_PCENET))
+ EMAC_Init(); //init EMAC if it is not already init'd
+ unsigned int regv;
+ regv = read_PHY(PHY_REG_EDCR);
+ write_PHY(PHY_REG_BMCR, regv & ~(1 << PHY_REG_EDCR_ENABLE));
+ regv = read_PHY(PHY_REG_EDCR);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PowerControl/EthernetPowerControl.h Thu Feb 07 17:43:19 2013 +0000 @@ -0,0 +1,299 @@ +/* mbed PowerControl Library + * Copyright (c) 2010 Michael Wei + */ + +#ifndef MBED_POWERCONTROL_ETH_H +#define MBED_POWERCONTROL_ETH_H + +#include "mbed.h" +#include "PowerControl.h" + +#define PHY_REG_BMCR_POWERDOWN 0xB +#define PHY_REG_EDCR_ENABLE 0xF + + +void EMAC_Init(); +static unsigned short read_PHY (unsigned int PhyReg); +static void write_PHY (unsigned int PhyReg, unsigned short Value); + +void PHY_PowerDown(void); +void PHY_PowerUp(void); +void PHY_EnergyDetect_Enable(void); +void PHY_EnergyDetect_Disable(void); + +//From NXP Sample Code .... Probably from KEIL sample code +/* EMAC Memory Buffer configuration for 16K Ethernet RAM. */ +#define NUM_RX_FRAG 4 /* Num.of RX Fragments 4*1536= 6.0kB */ +#define NUM_TX_FRAG 3 /* Num.of TX Fragments 3*1536= 4.6kB */ +#define ETH_FRAG_SIZE 1536 /* Packet Fragment size 1536 Bytes */ + +#define ETH_MAX_FLEN 1536 /* Max. Ethernet Frame Size */ + +/* EMAC variables located in 16K Ethernet SRAM */ +#define RX_DESC_BASE 0x20080000 +#define RX_STAT_BASE (RX_DESC_BASE + NUM_RX_FRAG*8) +#define TX_DESC_BASE (RX_STAT_BASE + NUM_RX_FRAG*8) +#define TX_STAT_BASE (TX_DESC_BASE + NUM_TX_FRAG*8) +#define RX_BUF_BASE (TX_STAT_BASE + NUM_TX_FRAG*4) +#define TX_BUF_BASE (RX_BUF_BASE + NUM_RX_FRAG*ETH_FRAG_SIZE) + +/* RX and TX descriptor and status definitions. */ +#define RX_DESC_PACKET(i) (*(unsigned int *)(RX_DESC_BASE + 8*i)) +#define RX_DESC_CTRL(i) (*(unsigned int *)(RX_DESC_BASE+4 + 8*i)) +#define RX_STAT_INFO(i) (*(unsigned int *)(RX_STAT_BASE + 8*i)) +#define RX_STAT_HASHCRC(i) (*(unsigned int *)(RX_STAT_BASE+4 + 8*i)) +#define TX_DESC_PACKET(i) (*(unsigned int *)(TX_DESC_BASE + 8*i)) +#define TX_DESC_CTRL(i) (*(unsigned int *)(TX_DESC_BASE+4 + 8*i)) +#define TX_STAT_INFO(i) (*(unsigned int *)(TX_STAT_BASE + 4*i)) +#define RX_BUF(i) (RX_BUF_BASE + ETH_FRAG_SIZE*i) +#define TX_BUF(i) (TX_BUF_BASE + ETH_FRAG_SIZE*i) + +/* MAC Configuration Register 1 */ +#define MAC1_REC_EN 0x00000001 /* Receive Enable */ +#define MAC1_PASS_ALL 0x00000002 /* Pass All Receive Frames */ +#define MAC1_RX_FLOWC 0x00000004 /* RX Flow Control */ +#define MAC1_TX_FLOWC 0x00000008 /* TX Flow Control */ +#define MAC1_LOOPB 0x00000010 /* Loop Back Mode */ +#define MAC1_RES_TX 0x00000100 /* Reset TX Logic */ +#define MAC1_RES_MCS_TX 0x00000200 /* Reset MAC TX Control Sublayer */ +#define MAC1_RES_RX 0x00000400 /* Reset RX Logic */ +#define MAC1_RES_MCS_RX 0x00000800 /* Reset MAC RX Control Sublayer */ +#define MAC1_SIM_RES 0x00004000 /* Simulation Reset */ +#define MAC1_SOFT_RES 0x00008000 /* Soft Reset MAC */ + +/* MAC Configuration Register 2 */ +#define MAC2_FULL_DUP 0x00000001 /* Full Duplex Mode */ +#define MAC2_FRM_LEN_CHK 0x00000002 /* Frame Length Checking */ +#define MAC2_HUGE_FRM_EN 0x00000004 /* Huge Frame Enable */ +#define MAC2_DLY_CRC 0x00000008 /* Delayed CRC Mode */ +#define MAC2_CRC_EN 0x00000010 /* Append CRC to every Frame */ +#define MAC2_PAD_EN 0x00000020 /* Pad all Short Frames */ +#define MAC2_VLAN_PAD_EN 0x00000040 /* VLAN Pad Enable */ +#define MAC2_ADET_PAD_EN 0x00000080 /* Auto Detect Pad Enable */ +#define MAC2_PPREAM_ENF 0x00000100 /* Pure Preamble Enforcement */ +#define MAC2_LPREAM_ENF 0x00000200 /* Long Preamble Enforcement */ +#define MAC2_NO_BACKOFF 0x00001000 /* No Backoff Algorithm */ +#define MAC2_BACK_PRESSURE 0x00002000 /* Backoff Presurre / No Backoff */ +#define MAC2_EXCESS_DEF 0x00004000 /* Excess Defer */ + +/* Back-to-Back Inter-Packet-Gap Register */ +#define IPGT_FULL_DUP 0x00000015 /* Recommended value for Full Duplex */ +#define IPGT_HALF_DUP 0x00000012 /* Recommended value for Half Duplex */ + +/* Non Back-to-Back Inter-Packet-Gap Register */ +#define IPGR_DEF 0x00000012 /* Recommended value */ + +/* Collision Window/Retry Register */ +#define CLRT_DEF 0x0000370F /* Default value */ + +/* PHY Support Register */ +#define SUPP_SPEED 0x00000100 /* Reduced MII Logic Current Speed */ +#define SUPP_RES_RMII 0x00000800 /* Reset Reduced MII Logic */ + +/* Test Register */ +#define TEST_SHCUT_PQUANTA 0x00000001 /* Shortcut Pause Quanta */ +#define TEST_TST_PAUSE 0x00000002 /* Test Pause */ +#define TEST_TST_BACKP 0x00000004 /* Test Back Pressure */ + +/* MII Management Configuration Register */ +#define MCFG_SCAN_INC 0x00000001 /* Scan Increment PHY Address */ +#define MCFG_SUPP_PREAM 0x00000002 /* Suppress Preamble */ +#define MCFG_CLK_SEL 0x0000001C /* Clock Select Mask */ +#define MCFG_RES_MII 0x00008000 /* Reset MII Management Hardware */ + +/* MII Management Command Register */ +#define MCMD_READ 0x00000001 /* MII Read */ +#define MCMD_SCAN 0x00000002 /* MII Scan continuously */ + +#define MII_WR_TOUT 0x00050000 /* MII Write timeout count */ +#define MII_RD_TOUT 0x00050000 /* MII Read timeout count */ + +/* MII Management Address Register */ +#define MADR_REG_ADR 0x0000001F /* MII Register Address Mask */ +#define MADR_PHY_ADR 0x00001F00 /* PHY Address Mask */ + +/* MII Management Indicators Register */ +#define MIND_BUSY 0x00000001 /* MII is Busy */ +#define MIND_SCAN 0x00000002 /* MII Scanning in Progress */ +#define MIND_NOT_VAL 0x00000004 /* MII Read Data not valid */ +#define MIND_MII_LINK_FAIL 0x00000008 /* MII Link Failed */ + +/* Command Register */ +#define CR_RX_EN 0x00000001 /* Enable Receive */ +#define CR_TX_EN 0x00000002 /* Enable Transmit */ +#define CR_REG_RES 0x00000008 /* Reset Host Registers */ +#define CR_TX_RES 0x00000010 /* Reset Transmit Datapath */ +#define CR_RX_RES 0x00000020 /* Reset Receive Datapath */ +#define CR_PASS_RUNT_FRM 0x00000040 /* Pass Runt Frames */ +#define CR_PASS_RX_FILT 0x00000080 /* Pass RX Filter */ +#define CR_TX_FLOW_CTRL 0x00000100 /* TX Flow Control */ +#define CR_RMII 0x00000200 /* Reduced MII Interface */ +#define CR_FULL_DUP 0x00000400 /* Full Duplex */ + +/* Status Register */ +#define SR_RX_EN 0x00000001 /* Enable Receive */ +#define SR_TX_EN 0x00000002 /* Enable Transmit */ + +/* Transmit Status Vector 0 Register */ +#define TSV0_CRC_ERR 0x00000001 /* CRC error */ +#define TSV0_LEN_CHKERR 0x00000002 /* Length Check Error */ +#define TSV0_LEN_OUTRNG 0x00000004 /* Length Out of Range */ +#define TSV0_DONE 0x00000008 /* Tramsmission Completed */ +#define TSV0_MCAST 0x00000010 /* Multicast Destination */ +#define TSV0_BCAST 0x00000020 /* Broadcast Destination */ +#define TSV0_PKT_DEFER 0x00000040 /* Packet Deferred */ +#define TSV0_EXC_DEFER 0x00000080 /* Excessive Packet Deferral */ +#define TSV0_EXC_COLL 0x00000100 /* Excessive Collision */ +#define TSV0_LATE_COLL 0x00000200 /* Late Collision Occured */ +#define TSV0_GIANT 0x00000400 /* Giant Frame */ +#define TSV0_UNDERRUN 0x00000800 /* Buffer Underrun */ +#define TSV0_BYTES 0x0FFFF000 /* Total Bytes Transferred */ +#define TSV0_CTRL_FRAME 0x10000000 /* Control Frame */ +#define TSV0_PAUSE 0x20000000 /* Pause Frame */ +#define TSV0_BACK_PRESS 0x40000000 /* Backpressure Method Applied */ +#define TSV0_VLAN 0x80000000 /* VLAN Frame */ + +/* Transmit Status Vector 1 Register */ +#define TSV1_BYTE_CNT 0x0000FFFF /* Transmit Byte Count */ +#define TSV1_COLL_CNT 0x000F0000 /* Transmit Collision Count */ + +/* Receive Status Vector Register */ +#define RSV_BYTE_CNT 0x0000FFFF /* Receive Byte Count */ +#define RSV_PKT_IGNORED 0x00010000 /* Packet Previously Ignored */ +#define RSV_RXDV_SEEN 0x00020000 /* RXDV Event Previously Seen */ +#define RSV_CARR_SEEN 0x00040000 /* Carrier Event Previously Seen */ +#define RSV_REC_CODEV 0x00080000 /* Receive Code Violation */ +#define RSV_CRC_ERR 0x00100000 /* CRC Error */ +#define RSV_LEN_CHKERR 0x00200000 /* Length Check Error */ +#define RSV_LEN_OUTRNG 0x00400000 /* Length Out of Range */ +#define RSV_REC_OK 0x00800000 /* Frame Received OK */ +#define RSV_MCAST 0x01000000 /* Multicast Frame */ +#define RSV_BCAST 0x02000000 /* Broadcast Frame */ +#define RSV_DRIB_NIBB 0x04000000 /* Dribble Nibble */ +#define RSV_CTRL_FRAME 0x08000000 /* Control Frame */ +#define RSV_PAUSE 0x10000000 /* Pause Frame */ +#define RSV_UNSUPP_OPC 0x20000000 /* Unsupported Opcode */ +#define RSV_VLAN 0x40000000 /* VLAN Frame */ + +/* Flow Control Counter Register */ +#define FCC_MIRR_CNT 0x0000FFFF /* Mirror Counter */ +#define FCC_PAUSE_TIM 0xFFFF0000 /* Pause Timer */ + +/* Flow Control Status Register */ +#define FCS_MIRR_CNT 0x0000FFFF /* Mirror Counter Current */ + +/* Receive Filter Control Register */ +#define RFC_UCAST_EN 0x00000001 /* Accept Unicast Frames Enable */ +#define RFC_BCAST_EN 0x00000002 /* Accept Broadcast Frames Enable */ +#define RFC_MCAST_EN 0x00000004 /* Accept Multicast Frames Enable */ +#define RFC_UCAST_HASH_EN 0x00000008 /* Accept Unicast Hash Filter Frames */ +#define RFC_MCAST_HASH_EN 0x00000010 /* Accept Multicast Hash Filter Fram.*/ +#define RFC_PERFECT_EN 0x00000020 /* Accept Perfect Match Enable */ +#define RFC_MAGP_WOL_EN 0x00001000 /* Magic Packet Filter WoL Enable */ +#define RFC_PFILT_WOL_EN 0x00002000 /* Perfect Filter WoL Enable */ + +/* Receive Filter WoL Status/Clear Registers */ +#define WOL_UCAST 0x00000001 /* Unicast Frame caused WoL */ +#define WOL_BCAST 0x00000002 /* Broadcast Frame caused WoL */ +#define WOL_MCAST 0x00000004 /* Multicast Frame caused WoL */ +#define WOL_UCAST_HASH 0x00000008 /* Unicast Hash Filter Frame WoL */ +#define WOL_MCAST_HASH 0x00000010 /* Multicast Hash Filter Frame WoL */ +#define WOL_PERFECT 0x00000020 /* Perfect Filter WoL */ +#define WOL_RX_FILTER 0x00000080 /* RX Filter caused WoL */ +#define WOL_MAG_PACKET 0x00000100 /* Magic Packet Filter caused WoL */ + +/* Interrupt Status/Enable/Clear/Set Registers */ +#define INT_RX_OVERRUN 0x00000001 /* Overrun Error in RX Queue */ +#define INT_RX_ERR 0x00000002 /* Receive Error */ +#define INT_RX_FIN 0x00000004 /* RX Finished Process Descriptors */ +#define INT_RX_DONE 0x00000008 /* Receive Done */ +#define INT_TX_UNDERRUN 0x00000010 /* Transmit Underrun */ +#define INT_TX_ERR 0x00000020 /* Transmit Error */ +#define INT_TX_FIN 0x00000040 /* TX Finished Process Descriptors */ +#define INT_TX_DONE 0x00000080 /* Transmit Done */ +#define INT_SOFT_INT 0x00001000 /* Software Triggered Interrupt */ +#define INT_WAKEUP 0x00002000 /* Wakeup Event Interrupt */ + +/* Power Down Register */ +#define PD_POWER_DOWN 0x80000000 /* Power Down MAC */ + +/* RX Descriptor Control Word */ +#define RCTRL_SIZE 0x000007FF /* Buffer size mask */ +#define RCTRL_INT 0x80000000 /* Generate RxDone Interrupt */ + +/* RX Status Hash CRC Word */ +#define RHASH_SA 0x000001FF /* Hash CRC for Source Address */ +#define RHASH_DA 0x001FF000 /* Hash CRC for Destination Address */ + +/* RX Status Information Word */ +#define RINFO_SIZE 0x000007FF /* Data size in bytes */ +#define RINFO_CTRL_FRAME 0x00040000 /* Control Frame */ +#define RINFO_VLAN 0x00080000 /* VLAN Frame */ +#define RINFO_FAIL_FILT 0x00100000 /* RX Filter Failed */ +#define RINFO_MCAST 0x00200000 /* Multicast Frame */ +#define RINFO_BCAST 0x00400000 /* Broadcast Frame */ +#define RINFO_CRC_ERR 0x00800000 /* CRC Error in Frame */ +#define RINFO_SYM_ERR 0x01000000 /* Symbol Error from PHY */ +#define RINFO_LEN_ERR 0x02000000 /* Length Error */ +#define RINFO_RANGE_ERR 0x04000000 /* Range Error (exceeded max. size) */ +#define RINFO_ALIGN_ERR 0x08000000 /* Alignment Error */ +#define RINFO_OVERRUN 0x10000000 /* Receive overrun */ +#define RINFO_NO_DESCR 0x20000000 /* No new Descriptor available */ +#define RINFO_LAST_FLAG 0x40000000 /* Last Fragment in Frame */ +#define RINFO_ERR 0x80000000 /* Error Occured (OR of all errors) */ + +#define RINFO_ERR_MASK (RINFO_FAIL_FILT | RINFO_CRC_ERR | RINFO_SYM_ERR | \ + RINFO_LEN_ERR | RINFO_ALIGN_ERR | RINFO_OVERRUN) + +/* TX Descriptor Control Word */ +#define TCTRL_SIZE 0x000007FF /* Size of data buffer in bytes */ +#define TCTRL_OVERRIDE 0x04000000 /* Override Default MAC Registers */ +#define TCTRL_HUGE 0x08000000 /* Enable Huge Frame */ +#define TCTRL_PAD 0x10000000 /* Pad short Frames to 64 bytes */ +#define TCTRL_CRC 0x20000000 /* Append a hardware CRC to Frame */ +#define TCTRL_LAST 0x40000000 /* Last Descriptor for TX Frame */ +#define TCTRL_INT 0x80000000 /* Generate TxDone Interrupt */ + +/* TX Status Information Word */ +#define TINFO_COL_CNT 0x01E00000 /* Collision Count */ +#define TINFO_DEFER 0x02000000 /* Packet Deferred (not an error) */ +#define TINFO_EXCESS_DEF 0x04000000 /* Excessive Deferral */ +#define TINFO_EXCESS_COL 0x08000000 /* Excessive Collision */ +#define TINFO_LATE_COL 0x10000000 /* Late Collision Occured */ +#define TINFO_UNDERRUN 0x20000000 /* Transmit Underrun */ +#define TINFO_NO_DESCR 0x40000000 /* No new Descriptor available */ +#define TINFO_ERR 0x80000000 /* Error Occured (OR of all errors) */ + +/* DP83848C PHY Registers */ +#define PHY_REG_BMCR 0x00 /* Basic Mode Control Register */ +#define PHY_REG_BMSR 0x01 /* Basic Mode Status Register */ +#define PHY_REG_IDR1 0x02 /* PHY Identifier 1 */ +#define PHY_REG_IDR2 0x03 /* PHY Identifier 2 */ +#define PHY_REG_ANAR 0x04 /* Auto-Negotiation Advertisement */ +#define PHY_REG_ANLPAR 0x05 /* Auto-Neg. Link Partner Abitily */ +#define PHY_REG_ANER 0x06 /* Auto-Neg. Expansion Register */ +#define PHY_REG_ANNPTR 0x07 /* Auto-Neg. Next Page TX */ + +/* PHY Extended Registers */ +#define PHY_REG_STS 0x10 /* Status Register */ +#define PHY_REG_MICR 0x11 /* MII Interrupt Control Register */ +#define PHY_REG_MISR 0x12 /* MII Interrupt Status Register */ +#define PHY_REG_FCSCR 0x14 /* False Carrier Sense Counter */ +#define PHY_REG_RECR 0x15 /* Receive Error Counter */ +#define PHY_REG_PCSR 0x16 /* PCS Sublayer Config. and Status */ +#define PHY_REG_RBR 0x17 /* RMII and Bypass Register */ +#define PHY_REG_LEDCR 0x18 /* LED Direct Control Register */ +#define PHY_REG_PHYCR 0x19 /* PHY Control Register */ +#define PHY_REG_10BTSCR 0x1A /* 10Base-T Status/Control Register */ +#define PHY_REG_CDCTRL1 0x1B /* CD Test Control and BIST Extens. */ +#define PHY_REG_EDCR 0x1D /* Energy Detect Control Register */ + +#define PHY_FULLD_100M 0x2100 /* Full Duplex 100Mbit */ +#define PHY_HALFD_100M 0x2000 /* Half Duplex 100Mbit */ +#define PHY_FULLD_10M 0x0100 /* Full Duplex 10Mbit */ +#define PHY_HALFD_10M 0x0000 /* Half Duplex 10MBit */ +#define PHY_AUTO_NEG 0x3000 /* Select Auto Negotiation */ + +#define DP83848C_DEF_ADR 0x0100 /* Default PHY device address */ +#define DP83848C_ID 0x20005C90 /* PHY Identifier */ +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PowerControl/PowerControl.h Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,192 @@
+/* mbed PowerControl Library
+ * Copyright (c) 2010 Michael Wei
+ */
+
+#ifndef MBED_POWERCONTROL_H
+#define MBED_POWERCONTROL_H
+
+//shouldn't have to include, but fixes weird problems with defines
+#include "LPC1768/LPC17xx.h"
+
+//System Control Register
+// bit 0: Reserved
+// bit 1: Sleep on Exit
+#define LPC1768_SCR_SLEEPONEXIT 0x2
+// bit 2: Deep Sleep
+#define LPC1768_SCR_SLEEPDEEP 0x4
+// bit 3: Resereved
+// bit 4: Send on Pending
+#define LPC1768_SCR_SEVONPEND 0x10
+// bit 5-31: Reserved
+
+//Power Control Register
+// bit 0: Power mode control bit 0 (power-down mode)
+#define LPC1768_PCON_PM0 0x1
+// bit 1: Power mode control bit 1 (deep power-down mode)
+#define LPC1768_PCON_PM1 0x2
+// bit 2: Brown-out reduced power mode
+#define LPC1768_PCON_BODRPM 0x4
+// bit 3: Brown-out global disable
+#define LPC1768_PCON_BOGD 0x8
+// bit 4: Brown-out reset disable
+#define LPC1768_PCON_BORD 0x10
+// bit 5-7 : Reserved
+// bit 8: Sleep Mode Entry Flag
+#define LPC1768_PCON_SMFLAG 0x100
+// bit 9: Deep Sleep Entry Flag
+#define LPC1768_PCON_DSFLAG 0x200
+// bit 10: Power Down Entry Flag
+#define LPC1768_PCON_PDFLAG 0x400
+// bit 11: Deep Power Down Entry Flag
+#define LPC1768_PCON_DPDFLAG 0x800
+// bit 12-31: Reserved
+
+//"Sleep Mode" (WFI).
+inline void Sleep(void)
+{
+ __WFI();
+}
+
+//"Deep Sleep" Mode
+inline void DeepSleep(void)
+{
+ SCB->SCR |= LPC1768_SCR_SLEEPDEEP;
+ __WFI();
+}
+
+//"Power-Down" Mode
+inline void PowerDown(void)
+{
+ SCB->SCR |= LPC1768_SCR_SLEEPDEEP;
+ LPC_SC->PCON &= ~LPC1768_PCON_PM1;
+ LPC_SC->PCON |= LPC1768_PCON_PM0;
+ __WFI();
+ //reset back to normal
+ LPC_SC->PCON &= ~(LPC1768_PCON_PM1 | LPC1768_PCON_PM0);
+}
+
+//"Deep Power-Down" Mode
+inline void DeepPowerDown(void)
+{
+ SCB->SCR |= LPC1768_SCR_SLEEPDEEP;
+ LPC_SC->PCON |= LPC1768_PCON_PM1 | LPC1768_PCON_PM0;
+ __WFI();
+ //reset back to normal
+ LPC_SC->PCON &= ~(LPC1768_PCON_PM1 | LPC1768_PCON_PM0);
+}
+
+//shut down BOD during power-down/deep sleep
+inline void BrownOut_ReducedPowerMode_Enable(void)
+{
+ LPC_SC->PCON |= LPC1768_PCON_BODRPM;
+}
+
+//turn on BOD during power-down/deep sleep
+inline void BrownOut_ReducedPowerMode_Disable(void)
+{
+ LPC_SC->PCON &= ~LPC1768_PCON_BODRPM;
+}
+
+//turn off brown out circutry
+inline void BrownOut_Global_Disable(void)
+{
+ LPC_SC->PCON |= LPC1768_PCON_BOGD;
+}
+
+//turn on brown out circutry
+inline void BrownOut_Global_Enable(void)
+{
+ LPC_SC->PCON &= !LPC1768_PCON_BOGD;
+}
+
+//turn off brown out reset circutry
+inline void BrownOut_Reset_Disable(void)
+{
+ LPC_SC->PCON |= LPC1768_PCON_BORD;
+}
+
+//turn on brown outreset circutry
+inline void BrownOut_Reset_Enable(void)
+{
+ LPC_SC->PCON &= ~LPC1768_PCON_BORD;
+}
+//Peripheral Control Register
+// bit 0: Reserved
+// bit 1: PCTIM0: Timer/Counter 0 power/clock enable
+#define LPC1768_PCONP_PCTIM0 0x2
+// bit 2: PCTIM1: Timer/Counter 1 power/clock enable
+#define LPC1768_PCONP_PCTIM1 0x4
+// bit 3: PCUART0: UART 0 power/clock enable
+#define LPC1768_PCONP_PCUART0 0x8
+// bit 4: PCUART1: UART 1 power/clock enable
+#define LPC1768_PCONP_PCUART1 0x10
+// bit 5: Reserved
+// bit 6: PCPWM1: PWM 1 power/clock enable
+#define LPC1768_PCONP_PCPWM1 0x40
+// bit 7: PCI2C0: I2C interface 0 power/clock enable
+#define LPC1768_PCONP_PCI2C0 0x80
+// bit 8: PCSPI: SPI interface power/clock enable
+#define LPC1768_PCONP_PCSPI 0x100
+// bit 9: PCRTC: RTC power/clock enable
+#define LPC1768_PCONP_PCRTC 0x200
+// bit 10: PCSSP1: SSP interface 1 power/clock enable
+#define LPC1768_PCONP_PCSSP1 0x400
+// bit 11: Reserved
+// bit 12: PCADC: A/D converter power/clock enable
+#define LPC1768_PCONP_PCADC 0x1000
+// bit 13: PCCAN1: CAN controller 1 power/clock enable
+#define LPC1768_PCONP_PCCAN1 0x2000
+// bit 14: PCCAN2: CAN controller 2 power/clock enable
+#define LPC1768_PCONP_PCCAN2 0x4000
+// bit 15: PCGPIO: GPIOs power/clock enable
+#define LPC1768_PCONP_PCGPIO 0x8000
+// bit 16: PCRIT: Repetitive interrupt timer power/clock enable
+#define LPC1768_PCONP_PCRIT 0x10000
+// bit 17: PCMCPWM: Motor control PWM power/clock enable
+#define LPC1768_PCONP_PCMCPWM 0x20000
+// bit 18: PCQEI: Quadrature encoder interface power/clock enable
+#define LPC1768_PCONP_PCQEI 0x40000
+// bit 19: PCI2C1: I2C interface 1 power/clock enable
+#define LPC1768_PCONP_PCI2C1 0x80000
+// bit 20: Reserved
+// bit 21: PCSSP0: SSP interface 0 power/clock enable
+#define LPC1768_PCONP_PCSSP0 0x200000
+// bit 22: PCTIM2: Timer 2 power/clock enable
+#define LPC1768_PCONP_PCTIM2 0x400000
+// bit 23: PCTIM3: Timer 3 power/clock enable
+#define LPC1768_PCONP_PCQTIM3 0x800000
+// bit 24: PCUART2: UART 2 power/clock enable
+#define LPC1768_PCONP_PCUART2 0x1000000
+// bit 25: PCUART3: UART 3 power/clock enable
+#define LPC1768_PCONP_PCUART3 0x2000000
+// bit 26: PCI2C2: I2C interface 2 power/clock enable
+#define LPC1768_PCONP_PCI2C2 0x4000000
+// bit 27: PCI2S: I2S interface power/clock enable
+#define LPC1768_PCONP_PCI2S 0x8000000
+// bit 28: Reserved
+// bit 29: PCGPDMA: GP DMA function power/clock enable
+#define LPC1768_PCONP_PCGPDMA 0x20000000
+// bit 30: PCENET: Ethernet block power/clock enable
+#define LPC1768_PCONP_PCENET 0x40000000
+// bit 31: PCUSB: USB interface power/clock enable
+#define LPC1768_PCONP_PCUSB 0x80000000
+
+//Powers Up specified Peripheral(s)
+inline unsigned int Peripheral_PowerUp(unsigned int bitMask)
+{
+ return LPC_SC->PCONP |= bitMask;
+}
+
+//Powers Down specified Peripheral(s)
+inline unsigned int Peripheral_PowerDown(unsigned int bitMask)
+{
+ return LPC_SC->PCONP &= ~bitMask;
+}
+
+//returns if the peripheral is on or off
+inline bool Peripheral_GetStatus(unsigned int peripheral)
+{
+ return (LPC_SC->PCONP & peripheral) ? true : false;
+}
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotControl/MotionState.cpp Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,135 @@
+#include "MotionState.h"
+
+using namespace std;
+
+MotionState::MotionState()
+{
+ xposition = 0.0f;
+ yposition = 0.0f;
+ theta = 0.0f;
+ thetaCompass = 0.0f;
+ speed = 0.0f;
+ omega = 0.0f;
+
+ acceleration = ACCELERATION;
+ thetaAcceleration = THETA_ACCELERATION;
+}
+
+
+MotionState::MotionState(float xposition, float yposition, float theta, float speed, float omega)
+{
+ this->xposition = xposition;
+ this->yposition = yposition;
+ this->theta = theta;
+ this->speed = speed;
+ this->omega = omega;
+ acceleration = ACCELERATION;
+ thetaAcceleration = THETA_ACCELERATION;
+}
+
+MotionState::~MotionState()
+{
+
+}
+
+void MotionState::setState(float xposition, float yposition, float theta, float speed, float omega)
+{
+ this->xposition = xposition;
+ this->yposition = yposition;
+ this->theta = theta;
+ this->speed = speed;
+ this->omega = omega;
+}
+
+void MotionState::setState(MotionState* motionState)
+{
+ xposition = motionState->xposition;
+ yposition = motionState->yposition;
+ theta = motionState->theta;
+ speed = motionState->speed;
+ omega = motionState->omega;
+}
+
+void MotionState::setxPosition(float xposition)
+{
+ this->xposition = xposition;
+}
+
+float MotionState::getxPosition()
+{
+ return xposition;
+}
+
+void MotionState::setyPosition(float yposition)
+{
+ this->yposition = yposition;
+}
+
+float MotionState::getyPosition()
+{
+ return yposition;
+}
+
+void MotionState::setTheta(float theta)
+{
+ this->theta = theta;
+}
+
+float MotionState::getTheta()
+{
+ return theta;
+}
+
+void MotionState::setSpeed(float speed)
+{
+ this->speed = speed;
+}
+
+float MotionState::getSpeed()
+{
+ return speed;
+}
+
+void MotionState::setOmega(float omega)
+{
+ this->omega = omega;
+}
+
+float MotionState::getOmega()
+{
+ return omega;
+}
+
+void MotionState::setAcceleration(float acceleration)
+{
+ this->acceleration = acceleration;
+}
+
+float MotionState::getAcceleration()
+{
+ return acceleration;
+}
+
+void MotionState::setThetaAcceleration(float thetaAcceleration)
+{
+ this->thetaAcceleration = thetaAcceleration;
+}
+
+float MotionState::getThetaAcceleration()
+{
+ return thetaAcceleration;
+}
+
+void MotionState::increment(float desiredSpeed, float desiredOmega, float period)
+{
+ float acceleration = (desiredSpeed-speed)/period;
+ if (acceleration < -this->acceleration) acceleration = -this->acceleration;
+ else if (acceleration > this->acceleration) acceleration = this->acceleration;
+
+ float thetaAcceleration = (desiredOmega-omega)/period;
+ if (thetaAcceleration < -this->thetaAcceleration) thetaAcceleration = -this->thetaAcceleration;
+ else if (thetaAcceleration > this->thetaAcceleration) thetaAcceleration = this->thetaAcceleration;
+
+ speed += acceleration * period;
+ omega += thetaAcceleration * period;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotControl/MotionState.h Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,177 @@
+#ifndef _MOTION_STATE_H_
+#define _MOTION_STATE_H_
+
+#include <cmath>
+#include "defines.h"
+
+/**
+ * @author Christian Burri
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
+ * All rights reserved.
+ *
+ * @section DESCRIPTION
+ *
+ * ?????
+ *
+ */
+class MotionState
+{
+
+private:
+
+ float acceleration;
+ float thetaAcceleration;
+
+public:
+
+ /** The xposition value of this motion state. */
+ float xposition;
+ /** The yposition value of this motion state. */
+ float yposition;
+ /** The angle of this motion state. */
+ float theta;
+ /** The angle of this motion state from the compass. */
+ float thetaCompass;
+ /** The speed value of this motion state. */
+ float speed;
+ /** The speed rotational value of this motion state. */
+ float omega;
+
+ /**
+ * Creates a <code>MotionState</code> object.
+ * The values for position and speed are set to 0.
+ */
+ MotionState();
+
+ /**
+ * Creates a <code>MotionState</code> object with given values for position and speed.
+ * @param xposition the initial position value of this motion state, given in [m].
+ * @param yposition the initial position value of this motion state, given in [m].
+ * @param theta the initial angle value of this motion state, given in [rad].
+ * @param speed the initial speed value of this motion state, given in [m/s].
+ * @param omega the initial omega speed value of this motion state, given in [rad/s].
+ */
+ MotionState(float xposition, float yposition, float theta, float speed, float omega);
+
+ /**
+ * Destructor of the Object to destroy the Object.
+ **/
+ virtual ~MotionState();
+
+ /**
+ * Sets the values for xPosition, yPostion and angle.
+ * @param xposition the initial position value of this motion state, given in [m].
+ * @param yposition the initial position value of this motion state, given in [m].
+ * @param theta the initial angle value of this motion state, given in [rad].
+ * @param speed the initial speed value of this motion state, given in [m/s].
+ * @param omega the initial omega speed value of this motion state, given in [rad/s].
+ */
+ void setState(float xposition, float yposition, float theta, float speed, float omega);
+
+ /**
+ * Sets the values for xPosition, yPostion and angle.
+ * @param motionState another <code>MotionState</code> object to copy the state values from.
+ */
+ void setState(MotionState* motionState);
+
+ /**
+ * Sets the X-position value.
+ * @param the desired xposition value of this motion state, given in [m].
+ */
+ void setxPosition(float position);
+
+ /**
+ * Gets the X-position value.
+ * @return the xposition value of this motion state, given in [m].
+ */
+ float getxPosition();
+
+ /**
+ * Sets the Y-position value.
+ * @param the desired yposition value of this motion state, given in [m].
+ */
+ void setyPosition(float yposition);
+
+ /**
+ * Gets the Y-position value.
+ * @return the xposition value of this motion state, given in [m].
+ */
+ float getyPosition();
+
+ /**
+ * Sets the theta value.
+ * @param the desired theta value of this motion state, given in [m].
+ */
+ void setTheta(float theta);
+
+ /**
+ * Gets the angle value.
+ * @return the theta value of this motion state, given in [rad].
+ */
+ float getTheta();
+
+ /**
+ * Sets the speed value.
+ * @param speed the desired speed value of this motion state, given in [m/s].
+ */
+ void setSpeed(float speed);
+
+ /**
+ * Gets the speed value.
+ * @return the speed value of this motion state, given in [m/s].
+ */
+ float getSpeed();
+
+ /**
+ * Sets the omega value.
+ * @param omega the desired omega value of this motion state, given in [rad/s].
+ */
+ void setOmega(float omega);
+
+ /**
+ * Gets the omega value.
+ * @return the omega value of this motion state, given in [rad/s].
+ */
+ float getOmega();
+
+ /**
+ * Sets the maximum acceleration value.
+ * @param acceleration the maximum acceleration value to use for the calculation
+ * of the motion trajectory, given in [m/s²].
+ */
+ void setAcceleration(float acceleration);
+
+ /**
+ * Gets the maximum acceleration value.
+ * @return the maximum acceleration value used for the calculation
+ * of the motion trajectory, given in [m/s²].
+ */
+ float getAcceleration();
+
+ /**
+ * Sets the maximum acceleration value of rotation.
+ * @param acceleration the maximum acceleration value to use for the calculation
+ * of the motion trajectory, given in [rad/s²].
+ */
+ void setThetaAcceleration(float thetaAcceleration);
+
+ /**
+ * Gets the maximum acceleration value of rotation.
+ * @return the maximum acceleration value used for the calculation
+ * of the motion trajectory, given in [rad/s²].
+ */
+ float getThetaAcceleration();
+
+ /**
+ * Increments the current motion state towards a given desired speed.
+ * @param desiredSpeed the desired speed given in [m/s].
+ * @param desiredOmega the desired omega given in [rad/s].
+ * @param period the time period to increment the motion state for, given in [s].
+ */
+ void increment(float desiredSpeed, float desiredOmega, float period);
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotControl/RobotControl.cpp Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,210 @@
+#include "RobotControl.h"
+
+using namespace std;
+
+RobotControl::RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, float period) : Task(period)
+{
+ /* get peripherals */
+ this->motorControllerLeft = motorControllerLeft;
+ this->motorControllerRight = motorControllerRight;
+ this->compass = compass;
+ this->period = period;
+
+ /* initialize peripherals */
+ motorControllerLeft->enable(false);
+ motorControllerRight->enable(false);
+
+ /* initialize remaining state values */
+ speed = 0.0f;
+ omega = 0.0f;
+
+ motorControllerLeft->setPulses(0);
+ motorControllerRight->setPulses(0);
+
+ Desired.setAcceleration(ACCELERATION);
+ Desired.setThetaAcceleration(THETA_ACCELERATION);
+
+}
+
+RobotControl::~RobotControl()
+{
+
+}
+
+void RobotControl::setEnable(bool enable)
+{
+ motorControllerLeft->enable(enable);
+ motorControllerRight->enable(enable);
+}
+
+bool RobotControl::isEnabled()
+{
+ return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled());
+}
+
+void RobotControl::setAcceleration(float acc)
+{
+ Desired.setAcceleration(acc);
+
+}
+
+void RobotControl::setThetaAcceleration(float acc)
+{
+ Desired.setThetaAcceleration(acc);
+}
+
+void RobotControl::setDesiredSpeed(float speed)
+{
+ this->speed = speed;
+}
+
+void RobotControl::setDesiredOmega(float omega)
+{
+ this->omega = omega;
+}
+
+void RobotControl::setxPosition(float position)
+{
+ Desired.xposition = position;
+}
+
+void RobotControl::setyPosition(float position)
+{
+ Desired.yposition = position;
+}
+
+void RobotControl::setTheta(float theta)
+{
+ Desired.theta = theta;
+}
+
+float RobotControl::getDesiredSpeed()
+{
+ return speed;
+}
+
+float RobotControl::getActualSpeed()
+{
+ return Actual.speed;
+}
+
+float RobotControl::getDesiredOmega()
+{
+ return omega;
+}
+
+float RobotControl::getActualOmega()
+{
+ return Actual.omega;
+}
+
+float RobotControl::getxActualPosition()
+{
+ return Actual.getxPosition();
+}
+
+float RobotControl::getxPositionError()
+{
+ return Desired.getxPosition()-Actual.getxPosition();
+}
+
+float RobotControl::getyActualPosition()
+{
+ return Actual.getyPosition();
+}
+
+float RobotControl::getyPositionError()
+{
+ return Desired.getyPosition()-Actual.getyPosition();
+}
+
+float RobotControl::getActualTheta()
+{
+ return Actual.getTheta();
+}
+
+float RobotControl::getThetaError()
+{
+ return Desired.getTheta()-Actual.getTheta();
+}
+
+void RobotControl::setAllToZero(float xZeroPos, float yZeroPos)
+{
+ Actual.setState(xZeroPos, yZeroPos, 0.0f, 0.0f, 0.0f);
+ Desired.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+ stateLeft.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+ stateRight.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+ speed = 0.0f;
+ omega = 0.0f;
+}
+
+
+void RobotControl::run()
+{
+ /* motion planning */
+ if (isEnabled()) {
+ Desired.increment(speed, omega, period);
+ } else {
+ speed = 0.0f;
+ omega = 0.0f;
+ Desired.setState(&Actual);
+ }
+
+ /* position calculation */
+
+ /* Set the state of speed from Left und Right Wheel*/
+ stateLeft.speed = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;
+ stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;
+
+ /* translational speed of the Robot (average) */
+ Actual.speed = ( stateRight.speed + stateLeft.speed ) / 2.0f;
+
+ /* rotational speed of the Robot */
+ Actual.omega = ( stateRight.speed - stateLeft.speed ) / WHEEL_DISTANCE;
+
+ /* rotational theta of the Robot */
+ Actual.theta += Actual.omega * period;
+ if(Actual.theta <= -PI) {
+ Actual.theta += 2* PI;
+ } else if (Actual.theta > PI) {
+ Actual.theta -= 2* PI;
+ } else {
+ //nothing
+ }
+ Actual.theta += Actual.omega * period;
+ Actual.thetaCompass = compass->getFilteredAngle();
+
+ /* translational X and Y Position. integrate the speed with the time */
+ // Actual.xposition += (Actual.speed * period * sin(Actual.theta));
+ // Actual.yposition += (Actual.speed * period * cos(Actual.theta));
+
+ /* translational X and Y Position. integrate the speed with the time theta from compass */
+ Actual.xposition += - (Actual.speed * period * sin(Actual.thetaCompass));
+ Actual.yposition += (Actual.speed * period * cos(Actual.thetaCompass));
+
+
+
+
+ /* postition control calculate */
+
+ rho = sqrt( pow(Actual.xposition,2) + pow(Actual.yposition,2));
+
+
+
+
+
+ /* motor control */
+ if (isEnabled()) {
+
+ motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS * PI) );
+ motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * PI) );
+
+
+ } else {
+
+ motorControllerLeft->setVelocity(0.0f);
+ motorControllerRight->setVelocity(0.0f);
+
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotControl/RobotControl.h Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,198 @@
+#ifndef _ROBOT_CONTROL_H_
+#define _ROBOT_CONTROL_H_
+
+#include <cmath>
+#include "mbed.h"
+#include "MaxonESCON.h"
+#include "MotionState.h"
+#include "Task.h"
+#include "HMC5883L.h"
+#include "HMC6352.h"
+#include "defines.h"
+
+/**
+ * @author Christian Burri
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
+ * All rights reserved.
+ *
+ * @section DESCRIPTION
+ *
+ * This class controls the position and orientation of the robot. It has
+ * a run loop that is called periodically. This run loop reads the actual
+ * positions of the wheels, calculates the actual position and orientation
+ * of the robot, calculates to move the robot,
+ * and writes these velocity values to the motor servo drives.
+ * This class offers methods to enable or disable the controller, and to set
+ * the desired translational and rotational speed values of the robot.
+ */
+
+class RobotControl : public Task
+{
+
+private:
+
+ MaxonESCON* motorControllerLeft;
+ MaxonESCON* motorControllerRight;
+ HMC6352* compass;
+ AnalogIn* battery;
+ MotionState Desired;
+ MotionState Actual;
+ MotionState stateLeft;
+ MotionState stateRight;
+ float period;
+ float speed;
+ float omega;
+
+ float rho; /* Distance to goal [m]*/
+ float gamma; /* Angle of the start position to goal position [rad]*/
+
+
+
+public:
+
+ /**
+ * Creates a robot control object and initializes all private
+ * state variables.
+ * @param motorControllerLeft a reference to the servo drive for the left motor.
+ * @param motorControllerRight a reference to the servo drive for the right motor.
+ * @param compass Modul HMC5883L
+ * @param period the sampling period of the run loop of this controller, given in [s].
+ */
+ RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, float period);
+
+ /**
+ * Destructor of the Object to destroy the Object.
+ **/
+ virtual ~RobotControl();
+
+ /**
+ * Enables or disables the servo drives of the motors.
+ * @param enable if <code>true</code> enables the drives, <code>false</code> otherwise
+ * the servo drives are shut down.
+ */
+ void setEnable(bool enable);
+
+ /**
+ * Tests if the servo drives of the motors are enabled.
+ * @return <code>true</code> if the drives are enabled, <code>false</code> otherwise.
+ */
+ bool isEnabled();
+
+ /**
+ * Sets the maximum acceleration value.
+ * @param acceleration the maximum acceleration value to use for the calculation
+ * of the motion trajectory, given in [m/s²].
+ */
+ void setAcceleration(float acc);
+
+ /**
+ * Sets the maximum acceleration value of rotation.
+ * @param acceleration the maximum acceleration value to use for the calculation
+ * of the motion trajectory, given in [rad/s²].
+ */
+ void setThetaAcceleration(float acc);
+
+ /**
+ * Sets the desired translational speed of the robot.
+ * @param speed the desired speed, given in [m/s].
+ */
+ void setDesiredSpeed(float speed);
+
+ /**
+ * Sets the desired rotational speed of the robot.
+ * @param speed the desired speed, given in [rad/s].
+ */
+ void setDesiredOmega(float omega);
+
+ /**
+ * Sets the desired X- position of the robot.
+ * @param xpostion the desired position, given in [m].
+ */
+ void setxPosition(float position);
+
+ /**
+ * Sets the desired Y-position of the robot.
+ * @param ypostion the desired position, given in [m].
+ */
+ void setyPosition(float position);
+
+ /**
+ * Sets the angle of the robot.
+ * @param theta the desired angle, given in [rad].
+ */
+ void setTheta(float theta);
+
+ /**
+ * Gets the desired translational speed of the robot.
+ * @return the desired speed, given in [m/s].
+ */
+ float getDesiredSpeed();
+
+ /**
+ * Gets the actual translational speed of the robot.
+ * @return the desired speed, given in [m/s].
+ */
+ float getActualSpeed();
+
+ /**
+ * Gets the desired rotational speed of the robot.
+ * @return the desired speed, given in [rad/s].
+ */
+ float getDesiredOmega();
+
+ /**
+ * Gets the actual rotational speed of the robot.
+ * @return the desired speed, given in [rad/s].
+ */
+ float getActualOmega();
+
+ /**
+ * Gets the actual translational X-position of the robot.
+ * @return the actual position, given in [m].
+ */
+ float getxActualPosition();
+
+ /**
+ * Gets the X-position following error of the robot.
+ * @return the position following error, given in [m].
+ */
+ float getxPositionError();
+
+ /**
+ * Gets the actual translational Y-position of the robot.
+ * @return the actual position, given in [m].
+ */
+ float getyActualPosition();
+
+ /**
+ * Gets the Y-position following error of the robot.
+ * @return the position following error, given in [m].
+ */
+ float getyPositionError();
+
+ /**
+ * Gets the actual orientation of the robot.
+ * @return the orientation, given in [rad].
+ */
+ float getActualTheta();
+
+ /**
+ * Gets the orientation following error of the robot.
+ * @return the orientation following error, given in [rad].
+ */
+ float getThetaError();
+
+ /**
+ * Set all state to zero
+ * @param Sets the start X-position [m].
+ * @param Sets the start y-position [m].
+ */
+ void setAllToZero(float xZeroPos, float xZeroPos);
+
+ void run();
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/HMC5883L.lib Thu Feb 07 17:43:19 2013 +0000 @@ -0,0 +1,1 @@ +Sensors/HMC5883L#f4e750710ff6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/HMC5883L/HMC5883L.cpp Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,421 @@
+/**
+ * @author Jose R. Padron
+ * @author Used HMC6352 library developed by Aaron Berk as template
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * 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.
+ *
+ * @section DESCRIPTION
+ *
+ * Honeywell HMC5883Ldigital compass.
+ *
+ * Datasheet:
+ *
+ * http://www.ssec.honeywell.com/magnetic/datasheets/HMC5883L.pdf
+ */
+
+#include "HMC5883L.h"
+
+HMC5883L::HMC5883L(PinName sda, PinName scl, float period): Task(period)
+{
+ i2c_ = new I2C(sda, scl);
+ compass_basis_rad = 0;
+ measurementNumber = 0;
+ this->setAbsGain(0.1); //0.1
+ this->setRelGain(0.05); // 0.05
+ output[0] = 0;
+ output[1] = 0;
+ output[2] = 0;
+ setDefault();
+ calib = true;
+}
+
+void HMC5883L::write(int address, int data)
+{
+ char tx[2];
+
+ tx[0]=address;
+ tx[1]=data;
+
+ i2c_->write(HMC5883L_I2C_WRITE,tx,2);
+
+ wait_ms(100);
+}
+
+void HMC5883L::setSleepMode()
+{
+ write(HMC5883L_MODE, HMC5883L_SLEEP);
+}
+
+void HMC5883L::setDefault(void)
+{
+ write(HMC5883L_CONFIG_A,HMC5883L_50HZ_NORMAL);
+ write(HMC5883L_CONFIG_B,HMC5883L_1_0GA);
+ write(HMC5883L_MODE,HMC5883L_CONTINUOUS);
+ wait_ms(100);
+}
+
+void HMC5883L::getAddress(char *buffer)
+{
+ char rx[3];
+ char tx[1];
+ tx[0]=HMC5883L_IDENT_A;
+
+ i2c_->write(HMC5883L_I2C_WRITE, tx,1);
+
+ wait_ms(1);
+
+ i2c_->read(HMC5883L_I2C_READ,rx,3);
+
+ buffer[0]=rx[0];
+ buffer[1]=rx[1];
+ buffer[2]=rx[2];
+}
+
+void HMC5883L::setOpMode(int mode, int ConfigA, int ConfigB)
+{
+ write(HMC5883L_CONFIG_A,ConfigA);
+ write(HMC5883L_CONFIG_B,ConfigB);
+ write(HMC5883L_MODE,mode);
+}
+
+void HMC5883L::readData(int* getMag)
+{
+ char tx[1];
+ char rx[2];
+
+ tx[0]=HMC5883L_X_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ getMag[0]= (int)rx[0]<<8|(int)rx[1];
+
+ tx[0]=HMC5883L_Y_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ getMag[1]= (int)rx[0]<<8|(int)rx[1];
+
+ tx[0]=HMC5883L_Z_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ getMag[2]= (int)rx[0]<<8|(int)rx[1];
+}
+
+int HMC5883L::getMx()
+{
+ char tx[1];
+ char rx[2];
+
+ tx[0]=HMC5883L_X_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ return ((int)rx[0]<<8|(int)rx[1]);
+}
+
+int HMC5883L::getMy()
+{
+
+ char tx[1];
+ char rx[2];
+
+ tx[0]=HMC5883L_Y_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ return ((int)rx[0]<<8|(int)rx[1]);
+}
+
+int HMC5883L::getMz()
+{
+ char tx[1];
+ char rx[2];
+
+ tx[0]=HMC5883L_Z_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ return ((int)rx[0]<<8|(int)rx[1]);
+}
+
+double HMC5883L::getAngle()
+{
+ int get_mag[N] = {0};
+ double input[N] = {0};
+ double angle = 0;
+
+ readData(get_mag);
+ for( int i=0; i<N; i++ ) {
+ input[i] = (int16_t)get_mag[i];
+ }
+ angle = atan2( input[1], input[0] ) - compass_basis_rad;
+ if(angle <= -PI) {
+ return angle += 2* PI;
+ } else if (angle > PI) {
+ return angle -= 2* PI;
+ } else {
+ return angle;
+ }
+}
+
+void HMC5883L::calibrate_compass_basis_rad(void)
+{
+ int calib_loop = 1000;
+ double compass_basis_buf = {0};
+
+ for( int i=0; i<calib_loop; i++ ) {
+ compass_basis_buf += getFilteredAngle();
+ run(); // run for next value
+ }
+ compass_basis_buf = compass_basis_buf/calib_loop;
+ compass_basis_rad = compass_basis_buf;
+}
+
+double HMC5883L::getCompass_basis_rad(void)
+{
+ return compass_basis_rad;
+}
+
+void HMC5883L::setAbsGain( float gain )
+{
+ if (gain>=0)
+ abs_gain=gain;
+}
+
+void HMC5883L::setRelGain( float gain )
+{
+ if (gain>=0 && gain<=1)
+ rel_gain=gain;
+}
+
+void HMC5883L::setExtremes(float *min, float *max)
+{
+ x_min=min[0];
+ y_min=min[1];
+ z_min=min[2];
+ x_max=max[0];
+ y_max=max[1];
+ z_max=max[2];
+
+ //Disable initial points
+ measurementNumber = INITIAL_POINTS+1;
+ calib = false;
+}
+
+void HMC5883L::getExtremes(float *min, float *max)
+{
+ min[0]=x_min;
+ min[1]=y_min;
+ min[2]=z_min;
+ max[0]=x_max;
+ max[1]=y_max;
+ max[2]=z_max;
+}
+
+
+void HMC5883L::getFilteredArray(float *outputs)
+{
+ outputs[0] = output[0];
+ outputs[1] = output[1];
+ outputs[2] = output[2];
+}
+
+double HMC5883L::getFilteredAngle(void)
+{
+ double angle = 0;
+ angle = - atan2( output[1], output[0] ) - compass_basis_rad;
+ if(angle <= -PI) {
+ return angle += 2 * PI;
+ } else if (angle > PI) {
+ return angle -= 2 * PI;
+ } else {
+ return angle;
+ }
+}
+
+void HMC5883L::run(void)
+{
+ float x_avg, y_avg, z_avg, x_range, y_range, z_range;
+ bool x_zero, y_zero, z_zero;
+ float temp;
+
+ int get_mag[3] = {0};
+ float input[3] = {0};
+
+ readData(get_mag);
+ for( int i=0; i<N; i++ ) {
+ input[i] = (int16_t)get_mag[i];
+ }
+
+ if (measurementNumber==0) { //Initial measurement just presets everything
+ x_min=input[0];
+ x_max=input[0];
+ y_min=input[1];
+ y_max=input[1];
+ z_min=input[2];
+ z_max=input[2];
+ measurementNumber++;
+ output[0] = input[0];
+ output[1] = input[1];
+ output[2] = input[2];
+ } else {
+ x_avg = (x_min+x_max)/2;
+ y_avg = (y_min+y_max)/2;
+ z_avg = (z_min+z_max)/2;
+
+
+ if (measurementNumber < INITIAL_POINTS) { //No abs gain used, no rel gain used, no range used
+ if (input[0]<x_min)
+ x_min = input[0];
+ else if (input[0]>x_max)
+ x_max = input[0];
+
+ if (input[1]<y_min)
+ y_min = input[1];
+ else if (input[1]>y_max)
+ y_max = input[1];
+
+ if (input[2]<z_min)
+ z_min = input[2];
+ else if (input[2]>z_max)
+ z_max = input[2];
+
+ //Return mags, since filter is still bad
+ output[0] = input[0];
+ output[1] = input[1];
+ output[2] = input[2];
+
+ measurementNumber++;
+
+ } else { //Now we should have enough data
+ x_range=x_max-x_min;
+ y_range=y_max-y_min;
+ z_range=z_max-z_min;
+
+ if(calib == true) { // when extremes sets, search no new extremas
+ //If measurement is a new extreme or:
+ if (input[0]<x_min) {
+ temp = rel_gain*(x_min - input[0]);
+ if (temp > abs_gain*(x_max-x_min))
+ temp = abs_gain*(x_max-x_min);
+ x_min = x_min - temp;
+ } else if (input[0]>x_max) {
+ temp = rel_gain*(input[0] - x_max);
+ if (temp > abs_gain*(x_max-x_min))
+ temp = abs_gain*(x_max-x_min);
+ x_max = x_max + temp;
+ }
+
+ if (input[1]<y_min) {
+ temp = rel_gain*(y_min - input[1]);
+ if (temp > abs_gain*(y_max-y_min))
+ temp = abs_gain*(y_max-y_min);
+ y_min = y_min - temp;
+ } else if (input[1]>y_max) {
+ temp = rel_gain*(input[1]-y_max);
+ if (temp > abs_gain*(y_max-y_min))
+ temp = abs_gain*(y_max-y_min);
+ y_max = y_max + temp;
+ }
+
+ if (input[2]<z_min) {
+ temp = rel_gain*(z_min - input[2]);
+ if (temp > abs_gain*(z_max-z_min))
+ temp = abs_gain*(z_max-z_min);
+ z_min = z_min - temp;
+ } else if (input[2]>z_max) {
+ temp = rel_gain*(input[2]-z_max );
+ if (temp > abs_gain*(z_max-z_min))
+ temp = abs_gain*(z_max-z_min);
+ z_max = z_max + temp;
+ }
+
+ //If measurement is near the zero of the other two axes
+ x_zero=false;
+ y_zero=false;
+ z_zero=false;
+ if (abs( input[0] - x_avg ) < (x_range * ZERO_RANGE))
+ x_zero=true;
+ if (abs( input[1] - y_avg ) < (y_range * ZERO_RANGE))
+ y_zero=true;
+ if (abs( input[2] - z_avg ) < (z_range * ZERO_RANGE))
+ z_zero=true;
+
+
+ if (x_zero && y_zero) {
+ if (input[2]>z_avg) {
+ temp = rel_gain*(input[2] - z_max);
+ if (abs(temp) > abs_gain*(z_max-z_min))
+ temp = sign(temp)*abs_gain*(z_max-z_min);
+ z_max = z_max + temp;
+ }
+ if (input[2]<z_avg) {
+ temp = rel_gain*(z_min - input[2]);
+ if (abs(temp) > abs_gain*(z_max-z_min))
+ temp = sign(temp)*abs_gain*(z_max-z_min);
+ z_min = z_min - temp;
+ }
+ }
+
+ if (x_zero && z_zero) {
+ if (input[1]>y_avg) {
+ temp = rel_gain*(input[1] - y_max);
+ if (abs(temp) > abs_gain*(y_max-y_min))
+ temp = sign(temp)*abs_gain*(y_max-y_min);
+ y_max = y_max + temp;
+ }
+ if (input[1]<y_avg) {
+ temp = rel_gain*(y_min - input[1]);
+ if (abs(temp) > abs_gain*(y_max-y_min))
+ temp = sign(temp)*abs_gain*(y_max-y_min);
+ y_min = y_min - temp;
+ }
+ }
+
+ if (y_zero && z_zero) {
+ if (input[0]>x_avg) {
+ temp = rel_gain*(input[0] - x_max);
+ if (abs(temp) > abs_gain*(x_max-x_min))
+ temp = sign(temp)*abs_gain*(x_max-x_min);
+ x_max = x_max + temp;
+ }
+ if (input[0]<x_avg) {
+ temp = rel_gain*(x_min - input[0]);
+ if (abs(temp) > abs_gain*(x_max-x_min))
+ temp = sign(temp)*abs_gain*(x_max-x_min);
+ x_min = x_min - temp;
+ }
+ }
+
+
+ }
+ //And now the actual filter part:
+ output[0] = 2* (input[0] - x_avg)/x_range;
+ output[1] = 2* (input[1] - y_avg)/y_range;
+ output[2] = 2* (input[2] - z_avg)/z_range;
+ }
+ }
+}
+
+
+float HMC5883L::sign(float input)
+{
+ if (input<0)
+ return -1.0;
+ else
+ return 1.0;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/HMC5883L/HMC5883L.h Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,303 @@
+/**
+ * @author Uwe Gartmann
+ * @author Used HMC5883L library developed by Jose R. Padron and Aaron Berk as template
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * 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.
+ *
+ * @section DESCRIPTION
+ *
+ * Honeywell HMC5883L digital compass.
+ *
+ * Datasheet:
+ *
+ * http://www.ssec.honeywell.com/magnetic/datasheets/HMC5883L.pdf
+ */
+
+#ifndef HMC5883L_H
+#define HMC5883L_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include "Task.h"
+#include "defines.h"
+
+/**
+ * Defines
+ */
+#define HMC5883L_I2C_ADDRESS 0x1E //7-bit address. 0x3C write, 0x3D read.
+#define HMC5883L_I2C_WRITE 0x3C
+#define HMC5883L_I2C_READ 0x3D
+
+//Values Config A
+#define HMC5883L_0_5HZ_NORMAL 0x00
+#define HMC5883L_0_5HZ_POSITIVE 0x01
+#define HMC5883L_0_5HZ_NEGATIVE 0x02
+
+#define HMC5883L_1HZ_NORMAL 0x04
+#define HMC5883L_1HZ_POSITIVE 0x05
+#define HMC5883L_1HZ_NEGATIVE 0x06
+
+#define HMC5883L_2HZ_NORMAL 0x08
+#define HMC5883L_2HZ_POSITIVE 0x09
+#define HMC5883L_2HZ_NEGATIVE 0x0A
+
+#define HMC5883L_5HZ_NORMAL 0x0C
+#define HMC5883L_5HZ_POSITIVE 0x0D
+#define HMC5883L_5HZ_NEGATIVE 0x0E
+
+#define HMC5883L_10HZ_NORMAL 0x10
+#define HMC5883L_10HZ_POSITIVE 0x11
+#define HMC5883L_10HZ_NEGATIVE 0x12
+
+#define HMC5883L_20HZ_NORMAL 0x14
+#define HMC5883L_20HZ_POSITIVE 0x15
+#define HMC5883L_20HZ_NEGATIVE 0x16
+
+#define HMC5883L_50HZ_NORMAL 0x18
+#define HMC5883L_50HZ_POSITIVE 0x19
+#define HMC5883L_50HZ_NEGATIVE 0x1A
+
+//Values Config B
+#define HMC5883L_0_7GA 0x00
+#define HMC5883L_1_0GA 0x20
+#define HMC5883L_1_5GA 0x40
+#define HMC5883L_2_0GA 0x60
+#define HMC5883L_3_2GA 0x80
+#define HMC5883L_3_8GA 0xA0
+#define HMC5883L_4_5GA 0xC0
+#define HMC5883L_6_5GA 0xE0
+
+//Values MODE
+#define HMC5883L_CONTINUOUS 0x00
+#define HMC5883L_SINGLE 0x01
+#define HMC5883L_IDLE 0x02
+#define HMC5883L_SLEEP 0x03
+
+#define HMC5883L_CONFIG_A 0x00
+#define HMC5883L_CONFIG_B 0x01
+#define HMC5883L_MODE 0x02
+#define HMC5883L_X_MSB 0x03
+#define HMC5883L_X_LSB 0x04
+#define HMC5883L_Z_MSB 0x05
+#define HMC5883L_Z_LSB 0x06
+#define HMC5883L_Y_MSB 0x07
+#define HMC5883L_Y_LSB 0x08
+#define HMC5883L_STATUS 0x09
+#define HMC5883L_IDENT_A 0x0A
+#define HMC5883L_IDENT_B 0x0B
+#define HMC5883L_IDENT_C 0x0C
+
+//Calibration
+#define N 3
+#define INITIAL_POINTS 100
+#define ZERO_RANGE 0.15
+
+/**
+ * Honeywell HMC5883L digital compass.
+ */
+class HMC5883L : public Task
+{
+
+private:
+
+ double compass_basis_rad; // Measurement algorithm from calibrate
+ double _angle; // Variable for getter methode
+ float output[3]; // output array calculate by run method
+ bool calib; // want allway calbirate???
+
+public:
+
+ /**
+ * Constructor.
+ *
+ * @param sda mbed pin to use for SDA line of I2C interface.
+ * @param scl mbed pin to use for SCL line of I2C interface.
+ */
+ HMC5883L(PinName sda, PinName scl, float period);
+
+ /**
+ * Enter into sleep mode.
+ *
+ */
+ void setSleepMode();
+
+ /**
+ * Set Device in Default Mode.
+ * HMC5883L_CONTINUOUS, HMC5883L_50HZ_NORMAL HMC5883L_1_0GA
+ */
+ void setDefault();
+
+ /**
+ * Read the memory location on the device which contains the address.
+ *
+ * @param Pointer to a buffer to hold the address value
+ * Expected H, 4 and 3.
+ */
+ void getAddress(char * address);
+
+ /**
+ * Set the operation mode.
+ *
+ * @param mode 0x00 -> Continuous
+ * 0x01 -> Single
+ * 0x02 -> Idle
+ * @param ConfigA values
+ * @param ConfigB values
+ */
+ void setOpMode(int mode, int ConfigA, int ConfigB);
+
+ /**
+ * Write to on the device.
+ *
+ * @param address Address to write to.
+ * @param data Data to write.
+ */
+
+ void write(int address, int data);
+
+ /**
+ * Get the output of all three axes.
+ *
+ * @param Pointer to a buffer to hold the magnetics value for the
+ * x-axis, y-axis and z-axis [in that order].
+ */
+ void readData(int* getMag);
+
+ /**
+ * Get the output of X axis.
+ *
+ * @return x-axis magnetic value
+ */
+ int getMx();
+
+ /**
+ * Get the output of Y axis.
+ *
+ * @return y-axis magnetic value
+ */
+ int getMy();
+
+ /**
+ * Get the output of Z axis.
+ *
+ * @return z-axis magnetic value
+ */
+ int getMz();
+
+ /**
+ * Get the output of the angel between x and y.
+ * At first calbirate!!!
+ * @return angle [rad]
+ */
+ double getAngle();
+
+ /**
+ * Get the current operation mode.
+ *
+ * @return Status register values
+ */
+ int getStatus(void);
+
+ /**
+ * Calibrate the compass to Zero Angle
+ */
+ void calibrate_compass_basis_rad(void);
+
+ /**
+ * Get the Basis from the Compass
+ * @return getcompass_basis_rad[rad]
+ */
+ double getCompass_basis_rad(void);
+
+ /** Sets the maximum absolute gain
+ *
+ * New data changes the calibration values, this setting limits the maximum rate this can change (constructor defaults it to 0.01)
+ * The first few datapoints (defined by INITIAL_POINTS) absGain (and relGain) is disabled to make sure the filter can work. If filter coefficients are manually added abs_gain wont be disabled.
+ * If absGain is set above 1000, it is also disabled.
+ *
+ * @param gain - a float that sets the maximum absolute gain, needs to be positive (zero turns of autocalibration)
+ */
+ void setAbsGain( float gain );
+
+ /** Sets the relative gain
+ *
+ * New data changes the calibration values, this setting changes how fast the calibration values will converge to their new value (constructor defaults it to 0.01).
+ *
+ * @param gain - a float that sets the maximum absolute gain, needs to be positive, smaller than one
+ */
+ void setRelGain( float gain );
+
+ /** The calibration works by calculating the extreme values for the axes, this function allows you to manually set them
+ *
+ * @param min - pointer to a float array with length three, which store the three minimum values (X-Y-Z)
+ * @param max - pointer to a float array with length three, which store the three maximum values (X-Y-Z)
+ */
+ void setExtremes(float *min, float *max);
+
+ /** The calibration works by calculating the extreme values for the axes, this function allows you to get them
+ *
+ * Doing calibration everytime from zero is not required normally, so it is useful to at the end of your program get these values,
+ * and set them next time you run the calibration. Storing them can for example be done on the local filesystem.
+ *
+ * @param min - pointer to a float array with length three, to store the three minimum values (X-Y-Z)
+ * @param max - pointer to a float array with length three, to store the three maximum values (X-Y-Z)
+ */
+ void getExtremes(float *min, float *max);
+
+ /**
+ * Return the magnetic filtered array
+ *
+ * @param output, float array with length three which contains the output data (X-Y-Z). Scaled to have on average a length of one.
+ */
+ void getFilteredArray(float *outputs);
+
+ /**
+ * Returns the filtered Angle
+ *
+ * @param angle from filtered run method.
+ */
+ double getFilteredAngle(void);
+
+ /**
+ * Runs the filter, takes the latest magnetometric inputs, and calculates the scaled/shifted outputs. Also updates the filter
+ *
+ * To prevent filter from updating, set relGain or absGain to zero.
+ */
+ void run(void);
+
+private:
+ float x_min, x_max, y_min, y_max, z_min, z_max;
+
+ float abs_gain, rel_gain;
+
+ int measurementNumber;
+
+ float sign(float input);
+
+ I2C* i2c_;
+
+};
+
+#endif /* HMC5883L_H */
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/HMC6352.lib Thu Feb 07 17:43:19 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/HMC6352/#cdaa0a63f3cf
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/Ping.lib Thu Feb 07 17:43:19 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/rosienej/code/Ping/#9c653265fc50
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Ping/Ping.cpp Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,54 @@
+#include "Ping.h"
+
+#include "mbed.h"
+
+Ping::Ping(PinName PING_PIN)
+ : _event(PING_PIN)
+ , _cmd(PING_PIN)
+ , _timer()
+ {
+ _event.rise(this,&Ping::_starts);
+ _event.fall(this,&Ping::_stops);
+ _SPEED_OF_SOUND_CM = 33;
+ }
+
+void Ping::_starts(void)
+{
+ _Valid = false; // start the timere, and invalidate the current time.
+ _Busy = true;
+ _timer.start();
+ _Time = _timer.read_us();
+}
+
+void Ping::_stops(void)
+{
+ _Valid = true; // When it stops, update the time
+ _Busy = false;
+ _Time = _timer.read_us()-_Time;
+}
+
+void Ping::send()
+{
+
+ _cmd.output();
+ _cmd.write(0); // see the ping documentation http://www.parallax.com/Portals/0/Downloads/docs/prod/acc/28015-PING-v1.6.pdf
+ wait_us(3);
+ _cmd.write(1);
+ wait_us(3);
+ _cmd.write(0);
+ _cmd.input();
+
+}
+void Ping::setSpeedOfSound(int SoS_ms )
+{
+ _SPEED_OF_SOUND_CM = SoS_ms;
+}
+
+int Ping::read_mm()
+// -1 means not valid.
+{
+ if(_Valid && ~_Busy)
+ return ((_Time*_SPEED_OF_SOUND_CM)/ 100);
+ else
+ return -1;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Ping/Ping.h Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,92 @@
+/* mbed Ping Library
+ * Copyright (c) 2007-2010 rosienej
+ *
+ * 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.
+ */
+
+ #ifndef MBED_PING_H
+ #define MBED_PING_H
+
+#include "mbed.h"
+/** Ping class, based on an InterruptIn pin, and a timer
+ * works with the parallax Ping))) sensor (www.parallax.com)
+ *
+ * @code
+ * // Continuously send pings and read the sensor
+ * #include "mbed.h"
+ * #include "Ping.h"
+ *
+ * Ping Pinger(p21);
+ *
+ * int main() {
+ * int range;
+
+ * while(1) {
+ *
+ * Pinger.Send();
+ * wait_ms(30);
+ * range = Pinger.Read_mm();
+ * }
+ * }
+ * @endcode
+ */
+class Ping {
+ public:
+ /** Create a Ping object connected to the specified InterruptIn pin
+ *
+ * @param PING_PIN InterruptIn pin to connect to
+ */
+ Ping(PinName PING_PIN);
+
+ /** Sends a Ping
+ *
+ * @param none
+ */
+ void send(void);
+
+ /** Set the speed of sound, default 33 cm/ms
+ *
+ * @param Speed of sound in centimeters per milliseconds
+ */
+ void setSpeedOfSound(int SoS_ms);
+
+ /** Read the result in milimeters
+ *
+ * @param none
+ */
+ int read_mm(void);
+
+ protected:
+
+ InterruptIn _event;
+ DigitalInOut _cmd;
+ Timer _timer;
+
+ bool _Valid;
+ bool _Busy;
+ int _Time;
+ int _SPEED_OF_SOUND_CM; /* in milliseconds */
+
+ void _starts(void);
+ void _stops(void);
+
+ };
+
+ #endif
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/StateDefines/State.cpp Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,169 @@
+#include "State.h"
+
+using namespace std;
+
+// File
+FILE *logp;
+
+// LocalFileSystem
+LocalFileSystem local("local");
+
+State::State(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, AnalogIn* battery, float period) : Task(period)
+{
+ /* get peripherals */
+ this->s = s;
+ this->robotControl = robotControl;
+ this->motorControllerLeft = motorControllerLeft;
+ this->motorControllerRight = motorControllerRight;
+ this->compass = compass;
+ this->battery =battery;
+ this->period = period;
+
+}
+
+
+State::~State() {}
+
+void State::initPlotFile(void)
+{
+ logp = fopen("/local/plots.txt", "w"); // only write
+ if(logp == NULL) {
+ exit(1);
+ } else {
+ fprintf(logp,
+ "Time[ms]\t"
+ "BatteryVoltage[V]\t"
+ "NumberOfPulsesLeft\t"
+ "NumberOfPulsesRight\t"
+ "VelocityLeft[m/s]\t"
+ "VelocityRight[m/s]\t"
+ "VelocityCar[m/s]\t"
+ "VelocityRotation[grad/s]\t"
+ "X-AxisCo-ordinate[m]\t"
+ "Y-AxisCo-ordinate[m]\t"
+ "X-AxisError[m]\t"
+ "X-AxisError[m]\t"
+ "AngleError[grad]\t"
+ "AngleCar[grad]\t"
+ "SetpointX-Axis[m]\t"
+ "SetpointY-Axis[m]\t"
+ "SetpointAngel[grad]\t"
+ "RightDistanceSensor[m]\t"
+ "CompassAngle[grad]\t"
+ "CompassX-Axis\t"
+ "CompassY-Axis\t"
+ "State\n");
+ }
+}
+
+
+void State::savePlotFile(state_t s)
+{
+ char buf[256];
+
+ sprintf(buf,"%d\t%f\t%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d",
+ s.millis,
+ s.voltageBattery,
+ s.leftPulses,
+ s.rightPulses,
+ s.leftVelocity,
+ s.rightVelocity,
+ s.velocity,
+ s.omega,
+ s.xAxis,
+ s.yAxis,
+ s.xAxisError,
+ s.yAxisError,
+ s.angleError,
+ s.angle,
+ s.setxAxis,
+ s.setyAxis,
+ s.setAngle,
+ s.rightDist,
+ s.compassAngle,
+ s.compassxAxis,
+ s.compassyAxis,
+ s.state
+ );
+
+ if (logp)
+ fprintf(logp, buf);
+ fprintf(logp, "\n"); // new line
+
+}
+
+void State::savePlotText(char text[])
+{
+ fprintf(logp, text);
+}
+
+
+void State::closePlotFile(void)
+{
+ fclose(logp);
+
+}
+
+float State::readBattery()
+{
+ return battery->read()*BAT_MULTIPLICATOR;
+}
+
+void State::setBatteryBit()
+{
+ if(s->voltageBattery < BAT_MIN) {
+ s->state |= STATE_UNDER;
+ } else {
+ s->state &= (~STATE_UNDER);
+ }
+}
+
+void State::setEnableLeftBit()
+{
+ if(motorControllerLeft->isEnabled()) {
+ s->state &= (~STATE_LEFT);
+ } else {
+ s->state |= STATE_LEFT;
+ }
+}
+
+void State::setEnableRightBit()
+{
+ if(motorControllerRight->isEnabled()) {
+ s->state &= (~STATE_RIGHT);
+ } else {
+ s->state |= STATE_RIGHT;
+ }
+}
+
+void State::startTimerFromZero()
+{
+ timer.reset();
+ timer.start();
+}
+
+
+void State::run()
+{
+ s->millis = timer.read_ms();
+ s->voltageBattery = readBattery();
+ s->leftPulses = - motorControllerLeft->getPulses();
+ s->rightPulses = motorControllerRight->getPulses();
+ s->leftVelocity = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;
+ s->rightVelocity = - motorControllerRight->getActualSpeed()* 2.0f * WHEEL_RADIUS * PI;
+ s->velocity = robotControl->getActualSpeed();
+ s->omega = robotControl->getActualOmega();
+ s->xAxis = robotControl->getxActualPosition();
+ s->yAxis = robotControl->getyActualPosition();
+ s->angle = robotControl->getActualTheta() * 180 / PI;
+ s->xAxisError = robotControl->getxPositionError();
+ s->yAxisError = robotControl->getyPositionError();
+ s->angleError = robotControl->getThetaError() * 180 / PI;
+ s->compassAngle = compass->getFilteredAngle() * 180 / PI;
+
+
+ setBatteryBit();
+ setEnableLeftBit();
+ setEnableRightBit();
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/StateDefines/State.h Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,113 @@
+#ifndef _STATE_H_
+#define _STATE_H_
+
+#include <cmath>
+#include "mbed.h"
+#include "MaxonESCON.h"
+#include "RobotControl.h"
+#include "Task.h"
+#include "HMC5883L.h"
+#include "HMC6352.h"
+#include "defines.h"
+
+/**
+ * @author Christian Burri
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
+ * All rights reserved.
+ *
+ * @section DESCRIPTION
+ *
+ * State is the main mechanism for communicating current realtime system state to
+ * the rest of the system for logging etc.
+ */
+
+class State : public Task
+{
+
+private:
+
+ state_t* s;
+ RobotControl* robotControl;
+ MaxonESCON* motorControllerLeft;
+ MaxonESCON* motorControllerRight;
+ HMC6352* compass;
+ AnalogIn* battery;
+ Timer timer;
+ float period;
+
+ float magout[3];
+
+
+
+public:
+
+
+ /**
+ * Creates a robot control object and initializes all private
+ * state variables.
+ * @param RobotControl Objekt to read the state.
+ * @param motorControllerLeft a reference to the servo drive for the left motor.
+ * @param motorControllerRight a reference to the servo drive for the right motor.
+ * @param compass Modul HMC5883L
+ * @param Analog Input Battery Voltage input
+ * @param period the sampling period of the run loop of this controller, given in [s].
+ */
+ State(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, AnalogIn* battery, float period);
+
+ /**
+ * Destructor of the Object to destroy the Object.
+ **/
+ virtual ~State();
+
+
+ /**
+ * Initzialize the File. Open the File plots.txt and set the title at first line
+ */
+ void initPlotFile(void);
+
+
+
+ /**
+ * Save the char to the file.
+ * For example at the end.
+ * Don't forget the \n at first.
+ */
+ void savePlotText(char text[]);
+
+
+ /**
+ * Close the File
+ */
+ void closePlotFile(void);
+
+
+ /**
+ * Return the Battery voltage
+ * state variables.
+ * @return Batterie Voltage [V]
+ */
+ float readBattery();
+
+ void startTimerFromZero();
+
+ /**
+ * Save the new state to a new line
+ */
+ void savePlotFile(state_t s);
+
+ void run();
+
+
+private:
+
+ void setBatteryBit();
+
+ void setEnableLeftBit();
+
+ void setEnableRightBit();
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/StateDefines/defines.h Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,122 @@
+#ifndef _DEFINES_H_
+#define _DEFINES_H_
+
+#include "mbed.h"
+
+//Physical dimensions.
+#define PI 3.141592654f
+
+// Motor #339282 EC 45 flat 30W and GEAR
+#define POLE_PAIRS 8u // 8
+#define GEAR 1.0f // Gear on the motor
+#define PULSES_PER_STEP 6u // 6 step for Hallsensor
+
+// Physical Dimension of the car
+#define WHEEL_RADIUS 0.042f // radius of the wheel, given in [m]
+#define WHEEL_DISTANCE 0.176f // Distance of the wheel, given in [m]
+
+// State Bits of the car
+#define STATE_STOP 1u // Bit0 = stop pressed
+#define STATE_UNDER 2u // Bit1 = Undervoltage battery
+#define STATE_LEFT 4u // Bit2 = left ESCON in error state
+#define STATE_RIGHT 8u // Bit3 = right ESCON in error state
+
+// ESCON Dimenstion
+#define ESCON_SET_FACTOR 100.0f // Speed Factor how set in the ESCON
+#define ESCON_GET_FACTOR 100.0f // Speed Factor how get in the ESCON
+
+// Start Defintition
+#define START_X_OFFSET -0.8f // Sets the start X-point [m]
+#define START_Y_OFFSET 0.8f // Sets the start Y-point [m]
+
+// Maximum Aceeleration
+#define ACCELERATION 0.25f // maximum translational acceleration, given in [m/s2]
+#define THETA_ACCELERATION 1.0f // maximum rotational acceleration, given in [rad/s2
+
+// LiPo Batterie
+#define BAT_MULTIPLICATOR 21.633333333f // R2 / (R1 + R2) = 0.153 R2= 10k , R1 = 1.8k
+// 1/0.153 = 6.555 ---> 3.3/1 * 6.555 = 21.6333333f
+#define BAT_MIN 17.5f // minium operate voltage [V] Battery Type: 1SP1P LG-18650 -->
+// nominal voltage 3.6V --> 5 batterys ==> 5 * 3.5V = 17.5V
+// Frequenz for the Task
+#define PERIOD_COMPASS 0.050f // 20Hz Rate for Compass HMC6352
+#define PERIOD_ROBOTCONTROL 0.001f // 1kHz Rate for Robot Control
+#define PERIOD_STATE 0.001f // 1kHz Rate for State Objekt
+#define PERIOD_ANDROID 0.1f // 10Hz Rate for State Objekt
+
+
+// Compass Maxima und Minima for the Filter
+#define SET_MAXIMAS_MINIMAS true // For Set the maximas und minimas when false the object search the maximas minimas by your own
+/*#define COMP_X_MAX 344.464996f // Maximum X-Range
+#define COMP_Y_MAX 238.751282f // Maximum Y-Range
+#define COMP_Z_MAX -266.899994f // Maximum Z-Range not used in this side
+#define COMP_X_MIN -90.412109f // Minimum X-Range
+#define COMP_Y_MIN -220.834808f // Minimum Y-Range
+#define COMP_Z_MIN -356.000000f // Minimun Z-Range not used in this side
+*/
+#define COMP_X_MAX 391.219910f // Maximum X-Range
+#define COMP_Y_MAX 382.915161f // Maximum Y-Range
+#define COMP_Z_MAX -237.855042f // Maximum Z-Range not used in this side
+#define COMP_X_MIN -169.952530f // Minimum X-Range
+#define COMP_Y_MIN -247.647675f // Minimum Y-Range
+#define COMP_Z_MIN -385.915009f // Minimun Z-Range not used in this side
+
+
+/**
+* struct state
+ * structure containing system sensor data
+ ****** System Status
+ ****** Data reported Motor
+ ****** Data reported Car
+ ****** Set Point Car
+ ****** measuring Position and angle
+ **/
+typedef struct state {
+ /** millis Time [ms]*/
+ int millis;
+ /** Battery voltage [V] */
+ float voltageBattery;
+ /** Number of pulses left */
+ int leftPulses;
+ /** Number of pulses right */
+ int rightPulses;
+ /** Velocity left [m/s] */
+ float leftVelocity;
+ /** Velocity right [m/s] */
+ float rightVelocity;
+ /** Velocity of the car [m/s] */
+ float velocity;
+ /** Velocity rotation [°/s] */
+ float omega;
+ /** X-Axis from co-ordinate [m] */
+ float xAxis;
+ /** Y-Axis from co-ordinate [m] */
+ float yAxis;
+ /** X-Axis Error [m] */
+ float xAxisError;
+ /** X-Axis Error [m] */
+ float yAxisError;
+ /** Angle Error [°] */
+ float angleError;
+ /** Angle from Car [°] */
+ float angle;
+ /** Setpoint X-Axis [m] */
+ float setxAxis;
+ /** Setpoint Y-Axis [m] */
+ float setyAxis;
+ /** Setpoint Angel [°] */
+ float setAngle;
+ /** Right Distance Sensor [m] */
+ float rightDist;
+ /** Compass Angle [°] */
+ float compassAngle;
+ /** Compass X-Axis */
+ float compassxAxis;
+ /** Compass Y-Axis */
+ float compassyAxis;
+ /** State of the Car **/
+ int state;
+} state_t;
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Task/Task.cpp Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,31 @@
+#include "Task.h"
+
+Task::Task(float period)
+{
+ this->period = period;
+}
+
+Task::~Task()
+{
+
+}
+
+float Task::getPeriod()
+{
+ return period;
+}
+
+void Task::start()
+{
+ ticker.attach(this, &Task::run, period);
+}
+
+void Task::stop()
+{
+ ticker.detach();
+}
+
+void Task::run()
+{
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Task/Task.h Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,77 @@
+#ifndef _TASK_H_
+#define _TASK_H_
+
+#include "mbed.h"
+
+/**
+ * @author Christian Burri
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
+ * All rights reserved.
+ * @section DESCRIPTION
+ * The <code>Task</code> class allows to install periodic, time-triggered
+ * tasks. An example of a simple user-defined task is given below:
+ * <pre><code>
+ * class MyTask : public Task {
+ * public:
+ * void run();
+ * };
+ *
+ * void MyTask::run() {
+ * <span style="color:#FF0000">// code to be executed periodically</span>
+ * }
+ * </code></pre>
+ * This task can then be created and started as follows:
+ * <pre><code>
+ * MyTask myTask(0.1); <span style="color:#FF0000">// period in seconds</span>
+ * myTask.start();
+ * ...
+ *
+ * myTask.stop();
+ * </code></pre>
+ */
+class Task
+{
+
+private:
+
+ /** specifiying the interval in seconds */
+ float period;
+ /** The Ticker interface is used to setup a recurring interrupt to repeatedly call a function at a specified rate. */
+ Ticker ticker;
+
+public:
+
+ /**
+ * Creates a task object with a given period.
+ * @param period the period of this task in seconds.
+ */
+ Task(float period);
+
+ virtual ~Task();
+
+ /**
+ * Gets the period of this task.
+ * @return the period in seconds.
+ */
+ float getPeriod();
+
+ /**
+ * Starts this task.
+ */
+ void start();
+
+ /**
+ * Stops this task.
+ */
+ void stop();
+
+ /**
+ * This method needs to be implemented by a user task.
+ */
+ virtual void run();
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,87 @@
+/*
+ * main.cpp
+ * Copyright (c) 2012, Pren Team1 HSLU T&A
+ * All rights reserved.
+ */
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// INCLUDES
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+#include "mbed.h"
+#include "math.h"
+#include "defines.h"
+#include "State.h"
+#include "HMC5883L.h"
+#include "HMC6352.h"
+#include "RobotControl.h"
+#include "Ping.h"
+#include "PowerControl/EthernetPowerControl.h"
+#include "Android.h"
+
+
+// LiPo Batterie
+AnalogIn battery(p15); // Battery check
+
+// compass
+//HMC5883L compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
+HMC6352 compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
+
+
+// ultrasonic sensor
+//Ping ultrasonic(p30);
+
+//Hallsensor
+//hall1, hall2, hall3
+Hallsensor hallLeft(p18, p17, p16);
+//hall1, hall2, hall3
+Hallsensor hallRight(p27, p28, p29);
+
+// Motors
+//enb, ready, pwm, actualSpeed, Hallsensor object
+MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft);
+//enb, ready, pwm, actualSpeed, Hallsensor object
+MaxonESCON rightMotor(p23, p22, p21, p20, &hallRight);
+
+// Robot Control
+RobotControl robotControl (&leftMotor, &rightMotor, &compass, PERIOD_ROBOTCONTROL);
+
+// Logging & State
+state_t s; // stuct state
+State state(&s, &robotControl, &leftMotor, &rightMotor, &compass, &battery, PERIOD_STATE);
+
+// Communication
+
+//Android android(&s, &robotControl, &leftMotor, &rightMotor, PERIOD_ANDROID);
+
+// PC USB communications
+Serial pc(USBTX, USBRX);
+
+DigitalOut myled(LED1);
+
+float magout[3] = {0};
+
+// LiPo Batterie
+float batterie_voltage;
+
+int main()
+{
+
+ /** Normal mbed power level for this setup is around 690mW
+ * assuming 5V used on Vin pin
+ * If you don't need networking...
+ * Power down Ethernet interface - saves around 175mW
+ * Also need to unplug network cable - just a cable sucks power
+ */
+ PHY_PowerDown();
+
+ robotControl.start();
+ compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+ compass.start();
+
+ robotControl.setEnable(false);
+ wait(1);
+ robotControl.setEnable(true);
+ wait(1);
+
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Feb 07 17:43:19 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/0954ebd79f59 \ No newline at end of file