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 11:775ebb69d5e1, committed 2013-04-05
- Comitter:
- chrigelburri
- Date:
- Fri Apr 05 10:58:42 2013 +0000
- Parent:
- 10:09ddb819fdcb
- Child:
- 12:235e318a414f
- Commit message:
- doku soweit gut ohne android
Changed in this revision
--- a/Actuators/Hallsensor.h Thu Apr 04 06:43:43 2013 +0000
+++ b/Actuators/Hallsensor.h Fri Apr 05 10:58:42 2013 +0000
@@ -6,12 +6,10 @@
/**
* @author Christian Burri
*
- * @section LICENSE
- *
- * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
+ * @copyright Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
* All rights reserved.
*
- * @section DESCRIPTION
+ * @brief
*
* Interface to count the Hallsensor input from a EC-Motor.
*
@@ -22,7 +20,7 @@
private:
/**
- * Update the pulse count.
+ * @brief 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.
@@ -41,7 +39,7 @@
public:
/**
- * Constructor of the class <code>Hallsensor</code>.
+ * @brief Constructor of the class <code>Hallsensor</code>.
*
* Reads the current values on Hall1 , Hall2 and Hall3 to determine the
* initial state.
@@ -54,19 +52,19 @@
Hallsensor(PinName hall1, PinName hall2, PinName hall3);
/**
- * Reset the encoder.
+ * @brief Reset the encoder.
* Sets the pulses and revolutions count to zero.
*/
void reset(void);
/**
- * Read the number of pulses recorded by the encoder.
+ * @brief Read the number of pulses recorded by the encoder.
* @return Number of pulses which have occured, given in [count]
*/
int getPulses(void);
/**
- * Read the number of revolutions recorded by the encoder.
+ * @brief Read the number of revolutions recorded by the encoder.
* @return Number of revolutions which have occured on the index channel.
*/
int getRevolutions(void);
--- a/Actuators/MaxonESCON.h Thu Apr 04 06:43:43 2013 +0000
+++ b/Actuators/MaxonESCON.h Fri Apr 05 10:58:42 2013 +0000
@@ -1,24 +1,19 @@
#ifndef _MAXON_ESCON_H_
#define _MAXON_ESCON_H_
-#include "mbed.h"
#include "Hallsensor.h"
#include "defines.h"
/**
* @author Christian Burri
*
- * @section LICENSE
- *
- * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
+ * @copyright Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
* All rights reserved.
*
- * @section DESCRIPTION
+ * @brief
*
* This class implements the driver for the Maxon ESCON servo driver.
- * For more information see on the Datasheet:
- *
- * Datasheet:
+ * For more information see the Datasheet:
* <a href="http://escon.maxonmotor.com">http://escon.maxonmotor.com</a>
*/
class MaxonESCON
@@ -26,22 +21,23 @@
private:
- /** To Enable the amplifier */
+ /** @brief To Enable the amplifier */
DigitalOut _enb;
- /** Duty Cycle to set the speed */
+ /** @brief Duty Cycle to set the speed */
PwmOut _pwm;
- /** Hallsensor Class */
+ /** @brief Hallsensor Class */
Hallsensor* _hall;
- /** Ready output from ESCON */
+ /** @brief Ready output from ESCON */
DigitalIn _isenb;
- /** Actual speed from ESCON analog Output 1 */
+ /** @brief Actual speed from ESCON analog Output 1 */
AnalogIn _actualSpeed;
- /** increment the Hallpattern */
+ /** @brief increment the Hallpattern */
int _pulses;
public:
- /** Create a motor control object.
+ /**
+ * @brief 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
@@ -54,42 +50,49 @@
PinName actualSpeed,
Hallsensor *hall);
- /** Set the speed of the motor with a pwm for 10%..90%.
+ /**
+ * @brief Set the speed of the motor with a pwm for 10%..90%.
* 50% PWM is 0rpm.
* Caclulate from [1/s] in [1/min] and the Factor of the ESCON.
* @param speed The speed of the motor as a normalised value, given in [1/s]
*/
void setVelocity(float speed);
- /**Return the speed from ESCON.
+ /**
+ * @brief Return the speed from ESCON.
* 0 rpm is defined in the Analog input as 1.65V
* @return speed of the motor, given in [1/s]
*/
float getActualSpeed(void);
- /** Set the period of the pwm duty cycle.
+ /**
+ * @brief Set the period of the pwm duty cycle.
* Wrapper for PwmOut::period()
* @param period Pwm duty cycle, given in [s].
*/
void period(float period);
- /** Set the Motor to a enable sate.
+ /**
+ * @brief Set the Motor to a enable sate.
* @param enb <code>false</code> for disable <code>true</code> for enable.
*/
void enable(bool enb);
- /**Tests if the servo drive is enabled.
- * @return <code>true</code> if the drive is enabled,
+ /**
+ * @brief 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 number of Pulses.
+ /**
+ * @brief Return the number of Pulses.
* @return Pulses, given in [count]
*/
int getPulses(void);
- /** Set the Pulses of the Motor, given in [count]
+ /**
+ * @brief Set the Pulses of the Motor, given in [count]
* @return Pulses, given in [count]
*/
int setPulses(int setPos);
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Android/android.cpp Fri Apr 05 10:58:42 2013 +0000
@@ -0,0 +1,190 @@
+#include "android.h"
+
+using namespace std;
+
+AdkTerm::AdkTerm() : AndroidAccessory(INBL,OUTL, "ARM", "mbed", "mbed Terminal", "0.1", "http://www.mbed.org", "0000000012345678")
+{
+
+};
+
+
+void AdkTerm::setupDevice()
+{
+ settick = false;
+ for (int i = 0; i<OUTL; i++) {
+ buffer[i] = 0;
+ }
+ bcount = 0;
+ //n.attach(this,&AdkTerm::AttachTick,5);
+ //tick.attach(this,&AdkTerm::onTick,0.1);
+}
+
+float AdkTerm::getx()
+{
+ return x/1000;
+}
+
+float AdkTerm::gety()
+{
+ return y/1000;
+}
+
+float AdkTerm::gett()
+{
+ return t * PI / 180;
+}
+
+/*void AdkTerm::AttachTick()
+{
+ if(!settick)tick.attach(this,&AdkTerm::onTick,0.04);
+ settick = true;
+}
+
+void AdkTerm::onTick()
+{
+ right = 1-Right;
+ left = 1-Left;
+ 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 AdkTerm::resetDevice()
+{
+ for (int i = 0; i<OUTL; i++) {
+ buffer[i] = 0;
+ }
+ bcount = 0;
+}
+
+void AdkTerm::Tokenize(const string& str,
+ vector<string>& tokens,
+ const string& delimiters /*= " "*/)
+{
+ // Skip delimiters at beginning.
+ string::size_type lastPos = str.find_first_not_of(delimiters, 0);
+ // Find first "non-delimiter".
+ string::size_type pos = str.find_first_of(delimiters, lastPos);
+
+ while (string::npos != pos || string::npos != lastPos) {
+ // Found a token, add it to the vector.
+ tokens.push_back(str.substr(lastPos, pos - lastPos));
+ // Skip delimiters. Note the "not_of"
+ lastPos = str.find_first_not_of(delimiters, pos);
+ // Find next "non-delimiter"
+ pos = str.find_first_of(delimiters, lastPos);
+ }
+}
+
+int AdkTerm::callbackRead(u8 *buf, int len)
+{
+ // convert buffer (unsigned char) to string
+ std::string str(reinterpret_cast<char*>(buf), len);
+
+ // new vector of strings
+ vector<string> tokens;
+
+ // tokenize the string with the semicolon separator
+ Tokenize(str, tokens, ";");
+ copy(tokens.begin(), tokens.end(), ostream_iterator<string>(cout, ", "));
+
+
+ if(tokens.size() > 2) {
+
+ //string to float
+ x = ::atof(tokens.at(0).c_str());
+ y = ::atof(tokens.at(1).c_str());
+ t = ::atof(tokens.at(2).c_str());
+
+ //pc.printf("Android x(%d): %f\n\r\n",len,x);
+ // pc.printf("Android y(%d): %f\n\r\n",len,y);
+ //pc.printf("Android t(%d): %f\n\r\n",len,t);
+ } else {
+ //pc.printf("Android sayys(%d): %s\n\r\n",len,str);
+ }
+
+// switch ( buf[0] ) {
+// case 'x':
+// if (buf[1] == ":"){pc.printf("X-Coordinate(%d): %s\n\r\n",len,buf)};
+// break;
+// case 'y':
+// if (buf[1] == ":"){pc.printf("Y-Coordinate(%d): %s\n\r\n",len,buf)};
+// break;
+// case 't':
+// if (buf[1] == ":"){pc.printf("Z-Coordinate(%d): %s\n\r\n",len,buf)};
+// break;
+// default:
+// pc.printf("Command not recognized (%d): %s\n\r\n\n\n",len,buf);
+// }
+
+
+ //AttachTick();
+
+ return 0;
+}
+
+// split: receives a char delimiter; returns a vector of strings
+// By default ignores repeated delimiters, unless argument rep == 1.
+
+int AdkTerm::callbackWrite()
+{
+ ind = false;
+ return 0;
+}
+
+void AdkTerm::serialIRQ()
+{
+ //buffer[bcount] = pc.getc();
+
+
+ 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);
+ ind = true;
+ this->write(wbuf,bcount);
+ bcount = 0;
+ } else {
+ if (buffer[bcount] != 0x08 && buffer[bcount] != 0x7F ) {
+ bcount++;
+ if (bcount == OUTL) {
+ bcount = 0;
+ }
+ } else {
+ bcount--;
+ }
+ }
+
+}
+
+
+// gehört nicht mehr zur klasse für zum testen.......!!!!!!!!
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Android/android.h Fri Apr 05 10:58:42 2013 +0000
@@ -0,0 +1,154 @@
+#ifndef _ANDROID_H_
+#define _ANDROID_H_
+
+#include "mbed.h"
+#include "AndroidAccessory.h"
+#include "defines.h"
+#include <string>
+#include <sstream>
+#include <vector>
+#include <iostream>
+#include <stdlib.h>
+
+/**
+ * @author Arno Galliker
+ *
+ * @subsection LICENSE
+ *
+ * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
+ * All rights reserved.
+ *
+ * @subsection DESCRIPTION
+ *
+ * This class implements communication with the android smartphone
+ * For more information see the Android ADK Cookbook:
+ * <a href="http://mbed.org/cookbook/mbed-with-Android-ADK">http://mbed.org/cookbook/mbed-with-Android-ADK</a>
+ *
+ * Originally created by p07gbar from work by Makoto Abe
+ */
+
+#define OUTL 100
+#define INBL 100
+
+class AdkTerm : public AndroidAccessory
+{
+
+private:
+
+ /** Attaches a tick to send messages over the USB buffer in a certain interval */
+ void AttachTick();
+
+ /** Method to call when a tick period is over */
+ void onTick();
+
+ /** Char buffer with a size of OUTL */
+ char buffer[OUTL];
+
+ /** Buffer count ??? */
+ int bcount;
+
+ /** Instance of ticker */
+ Ticker tick;
+
+ //float right,left,rl,ll;
+ //int tl,tr;
+
+ //Timeout n;
+
+ /** States if tick is on */
+ bool settick;
+
+ /** States if something is written to the buffer?? */
+ bool ind;
+
+ /** Desired position in meters for x-coordinate, given by android */
+ float x;
+
+ /** Desired position in meters for y-coordinate, given by android */
+ float y;
+
+ /** Desired position in degrees for theta, given by android */
+ float t;
+
+public:
+
+ /**
+ * Creates a <code>AdkTerm</code> object and initializes all private
+ * state variables. Constructor
+ */
+ AdkTerm();
+
+ /**
+ * Sets initial configurations and clears the buffer
+ *
+ */
+ void setupDevice();
+
+ /**
+ * Returns the desired position in meters for x-coordinate, given by android
+ * @return x-coordinate,given in [m]
+ */
+ float getx();
+
+ /**
+ * Returns the desired position in meters for y-coordinate, given by android
+ * @return y-coordinate,given in [m]
+ */
+ float gety();
+
+ /**
+ * Returns the esired position in degrees for theta, given by android
+ * @return y-coordinate,given in degrees [°]
+ */
+ float gett();
+
+private:
+
+ /**
+ *
+ *
+ */
+ //void AttachTick();
+
+ /**
+ *
+ *
+ */
+ // void onTick();
+
+ /**
+ * Clears the buffer and bcount
+ *
+ */
+ void resetDevice();
+
+ /**
+ * Takes an string, a vector of strings for the delimited tokens, and a with the
+ * @param str
+ * @param tokens
+ * @param delimiters
+ */
+ void Tokenize(const string& str, vector<string>& tokens, const string& delimiters = " ");
+
+ /**
+ *
+ * @param buf
+ * @param len
+ */
+ int callbackRead(u8 *buf, int len);
+
+ /**
+ *
+ *
+ */
+ int callbackWrite();
+
+ /**
+ *
+ *
+ */
+ void serialIRQ();
+
+};
+
+#endif
--- a/PowerControl/EthernetPowerControl.cpp Thu Apr 04 06:43:43 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,138 +0,0 @@
-#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
--- a/PowerControl/EthernetPowerControl.h Thu Apr 04 06:43:43 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,299 +0,0 @@ -/* 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
--- a/PowerControl/PowerControl.h Thu Apr 04 06:43:43 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,192 +0,0 @@
-/* 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
--- a/RobotControl/MotionState.cpp Thu Apr 04 06:43:43 2013 +0000
+++ b/RobotControl/MotionState.cpp Fri Apr 05 10:58:42 2013 +0000
@@ -7,12 +7,8 @@
xposition = 0.0f;
yposition = 0.0f;
theta = 0.0f;
- thetaCompass = 0.0f;
speed = 0.0f;
omega = 0.0f;
-
- acceleration = ACCELERATION;
- thetaAcceleration = THETA_ACCELERATION;
}
@@ -27,8 +23,6 @@
this->theta = theta;
this->speed = speed;
this->omega = omega;
- acceleration = ACCELERATION;
- thetaAcceleration = THETA_ACCELERATION;
}
MotionState::~MotionState()
@@ -108,38 +102,3 @@
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;
-}
--- a/RobotControl/MotionState.h Thu Apr 04 06:43:43 2013 +0000
+++ b/RobotControl/MotionState.h Fri Apr 05 10:58:42 2013 +0000
@@ -6,51 +6,39 @@
/**
* @author Christian Burri
*
- * @section LICENSE
- *
- * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
+ * @copyright Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
* All rights reserved.
*
- * @section DESCRIPTION
+ * @brief
*
* This help class is for calculate and save the actual or desired value.
* There have the setter and the getter methode to change the value
- * Is also possible to limit the translational and rotational speed
- * by the value acceleration and thetaAcceleration. Therefore
- * can adjust the ramp.
*
*/
class MotionState
{
-private:
-
- float acceleration;
- float thetaAcceleration;
-
public:
- /** The xposition value of this motion state. */
+ /** @brief The xposition value of this motion state. */
float xposition;
- /** The yposition value of this motion state. */
+ /** @brief The yposition value of this motion state. */
float yposition;
- /** The θ of this motion state. */
+ /** @brief The θ of this motion state. */
float theta;
- /** The θ of this motion state from the compass. */
- float thetaCompass;
- /** The speed value of this motion state. */
+ /** @brief The speed value of this motion state. */
float speed;
- /** The speed rotational value of this motion state. */
+ /** @brief The speed rotational value of this motion state. */
float omega;
/**
- * Creates a <code>MotionState</code> object.
+ * @brief 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.
+ * @brief 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 θ value of this motion state, given in [rad]
@@ -64,12 +52,12 @@
float omega);
/**
- * Destructor of the Object to destroy the Object.
+ * @brief Destructor of the Object to destroy the Object.
**/
virtual ~MotionState();
/**
- * Sets the values for xPosition, yPostion and θ.
+ * @brief Sets the values for xPosition, yPostion and θ.
* @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 θ value of this motion state, given in [rad]
@@ -83,108 +71,71 @@
float omega);
/**
- * Sets the values for X-Position, Y-Postion and θ.
+ * @brief Sets the values for X-Position, Y-Postion and θ.
* @param motionState another <code>MotionState</code> object to copy the state values from
*/
void setState(MotionState* motionState);
/**
- * Sets the X-position value.
+ * @brief Sets the X-position value.
* @param xposition the desired xposition value of this motion state, given in [m]
*/
void setxPosition(float xposition);
/**
- * Gets the X-position value.
+ * @brief Gets the X-position value.
* @return the xposition value of this motion state, given in [m]
*/
float getxPosition();
/**
- * Sets the Y-position value.
+ * @brief Sets the Y-position value.
* @param yposition the desired yposition value of this motion state, given in [m]
*/
void setyPosition(float yposition);
/**
- * Gets the Y-position value.
+ * @brief Gets the Y-position value.
* @return the xposition value of this motion state, given in [m]
*/
float getyPosition();
/**
- * Sets the θ value.
+ * @brief Sets the θ value.
* @param theta the desired θ value of this motion state, given in [rad]
*/
void setTheta(float theta);
/**
- * Gets the θ value.
+ * @brief Gets the θ value.
* @return the θ value of this motion state, given in [rad]
*/
float getTheta();
/**
- * Sets the speed value.
+ * @brief 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.
+ * @brief Gets the speed value.
* @return the speed value of this motion state, given in [m/s]
*/
float getSpeed();
/**
- * Sets the ω value.
+ * @brief Sets the ω value.
* @param omega the desired ω value of this motion state, given in [rad/s]
*/
void setOmega(float omega);
/**
- * Gets the ω value.
+ * @brief Gets the ω value.
* @return the ω 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<SUP>2</SUP>]
- */
- 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<SUP>2</SUP>]
- */
- float getAcceleration();
-
- /**
- * Sets the maximum acceleration value of rotation.
- * @param thetaAcceleration the maximum acceleration value to use for
- * the calculation of the motion trajectory, given in [rad/<SUP>2</SUP>]
- */
- 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/<SUP>2</SUP>]
- */
- 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 ω 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
--- a/RobotControl/RobotControl.cpp Thu Apr 04 06:43:43 2013 +0000
+++ b/RobotControl/RobotControl.cpp Fri Apr 05 10:58:42 2013 +0000
@@ -4,13 +4,11 @@
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 */
@@ -26,9 +24,6 @@
motorControllerLeft->setVelocity(0.0f);
motorControllerRight->setVelocity(0.0f);
-
- Desired.setAcceleration(ACCELERATION);
- Desired.setThetaAcceleration(THETA_ACCELERATION);
}
RobotControl::~RobotControl()
@@ -47,16 +42,6 @@
return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled());
}
-void RobotControl::setAcceleration(float acceleration)
-{
- Desired.setAcceleration(acceleration);
-}
-
-void RobotControl::setThetaAcceleration(float acceleration)
-{
- Desired.setThetaAcceleration(acceleration);
-}
-
void RobotControl::setDesiredSpeed(float speed)
{
this->speed = speed;
@@ -190,20 +175,14 @@
void RobotControl::run()
{
- ///// DAs kan glaub raus ab hier
-/////////////////////////////////////////////////////////7
- /* motion planning */
+// kann glaub wegggggg
if (isEnabled()) {
- ///// DAs kan glaub raus bis hier
- Desired.increment(speed, omega, period);
-
- ///// DAs kan glaub raus bis hier
-
} else {
speed = 0.0f;
omega = 0.0f;
Desired.setState(&Actual);
}
+ ////bis hieerrrrr
/* position calculation */
@@ -227,11 +206,6 @@
Actual.xposition += (Actual.speed * period * cos(Actual.theta));
Actual.yposition += (Actual.speed * period * sin(Actual.theta));
- // Actual.thetaCompass = compass->getFilteredAngle();
- /* translational X and Y Position. integrate the speed with the time theta from compass */
- // Actual.xposition += - (Actual.speed * period * cos(Actual.thetaCompass));
- // Actual.yposition += (Actual.speed * period * sin(Actual.thetaCompass));
-
/* motor control */
if ( isEnabled() && ( getDistanceError() >= MIN_DISTANCE_ERROR ) ) {
--- a/RobotControl/RobotControl.h Thu Apr 04 06:43:43 2013 +0000
+++ b/RobotControl/RobotControl.h Fri Apr 05 10:58:42 2013 +0000
@@ -1,23 +1,18 @@
#ifndef _ROBOT_CONTROL_H_
#define _ROBOT_CONTROL_H_
-#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 © 2013 HSLU Pren Team #1 Cruising Crêpe
+ * @copyright Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
* All rights reserved.
*
- * @section DESCRIPTION
+ * @brief
*
* This class controls the position of the robot. It has
* a run loop that is called periodically. This run loop reads the actual
@@ -35,8 +30,6 @@
MaxonESCON* motorControllerLeft;
MaxonESCON* motorControllerRight;
-// HMC6352* compass;
- AnalogIn* battery;
MotionState Desired;
MotionState Actual;
MotionState stateLeft;
@@ -46,7 +39,7 @@
float omega;
/**
- * Add ±2π when the range of
+ * @brief Add ±2π when the range of
* the radian is over +π or under -π.
* @param theta to check the value
* @return the value in the range from -π to +π
@@ -56,29 +49,26 @@
public:
/**
- * Creates a <code>Robot Control</code> object and
+ * @brief Creates a <code>Robot Control</code> 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
+ * @param period the sampling period of the run loop of
* this controller, given in [s]
*/
RobotControl(MaxonESCON* motorControllerLeft,
- MaxonESCON*
- motorControllerRight,
- /*HMC6352* compass,*/
+ MaxonESCON* motorControllerRight,
float period);
/**
- * Destructor of the Object to destroy the Object.
+ * @brief Destructor of the Object to destroy the Object.
**/
virtual ~RobotControl();
/**
- * Enables or disables the servo drives of the motors.
+ * @brief 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.
@@ -86,21 +76,21 @@
void setEnable(bool enable);
/**
- * Tests if the servo drives of the motors are enabled.
+ * @brief 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.
+ * @brief Sets the maximum acceleration value.
* @param acceleration the maximum acceleration value to use
* for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>]
*/
void setAcceleration(float acceleration);
/**
- * Sets the maximum acceleration value of rotation.
+ * @brief 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<SUP>2</SUP>]
@@ -108,55 +98,55 @@
void setThetaAcceleration(float acceleration);
/**
- * Sets the desired translational speed of the robot.
+ * @brief 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.
+ * @brief Sets the desired rotational speed of the robot.
* @param omega the desired rotational speed, given in [rad/s]
*/
void setDesiredOmega(float omega);
/**
- * Sets the desired X-position of the robot.
+ * @brief Sets the desired X-position of the robot.
* @param xposition the desired position, given in [m]
*/
void setDesiredxPosition(float xposition);
/**
- * Sets the desired Y-position of the robot.
+ * @brief Sets the desired Y-position of the robot.
* @param yposition the desired position, given in [m]
*/
void setDesiredyPosition(float yposition);
/**
- * Sets the desired θ of the robot.
+ * @brief Sets the desired θ of the robot.
* @param theta the desired θ, given in [rad]
*/
void setDesiredTheta(float theta);
/**
- * Get the desired X-position of the robot.
+ * @brief Get the desired X-position of the robot.
* @return xposition the desired position, given in [m]
*/
float getDesiredxPosition();
/**
- * Get the desired Y-position of the robot.
+ * @brief Get the desired Y-position of the robot.
* @return yposition the desired position, given in [m]
*/
float getDesiredyPosition();
/**
- * Get the desired θ of the robot.
+ * @brief Get the desired θ of the robot.
* @return theta the desired θ, given in [rad]
*/
float getDesiredTheta();
/**
- * Sets the desired Position and θ.
+ * @brief Sets the desired Position and θ.
* @param xposition the desired position, given in [m]
* @param yposition the desired position, given in [m]
* @param theta the desired θ, given in [rad]
@@ -166,92 +156,92 @@
float theta);
/**
- * Gets the desired θ of the goal point.
+ * @brief Gets the desired θ of the goal point.
* @return the desired θ, given in [rad]
*/
float getTheta();
/**
- * Gets the desired translational speed of the robot.
+ * @brief 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.
+ * @brief 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.
+ * @brief 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.
+ * @brief 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.
+ * @brief 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.
+ * @brief 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.
+ * @brief 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.
+ * @brief 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.
+ * @brief Gets the actual orientation of the robot.
* @return the orientation, given in [rad]
*/
float getActualTheta();
/**
- * Gets the orientation following error of the robot.
+ * @brief Gets the orientation following error of the robot.
* @return the orientation following error, given in [rad]
*/
float getThetaError();
/**
- * Gets the distance to disired point. Calculate with pythagoras.
+ * @brief Gets the distance to disired point. Calculate with pythagoras.
* @return distance to goal, given in [m]
*/
float getDistanceError();
/**
- * Gets the θ ot the pointing vector to the goal right
+ * @brief Gets the θ ot the pointing vector to the goal right
* the unicycle axis from actual point.
* @return theta to goal, given in [rad]
*/
float getThetaErrorToGoal();
/**
- * Gets the θ ot the pointing vector to the goal right the unicycle main axis.
+ * @brief Gets the θ ot the pointing vector to the goal right the unicycle main axis.
* @return theta from the goal, given in [rad]
*/
float getThetaGoal();
/**
- * Set all state to zero, except the X-position, y-position and θ.
+ * @brief Set all state to zero, except the X-position, y-position and θ.
* @param xZeroPos Sets the start X-position, given in [m]
* @param yZeroPos Sets the start y-position, given in [m]
* @param theta Sets the start θ, given in [rad]
@@ -260,7 +250,9 @@
float yZeroPos,
float theta);
-
+ /**
+ * @brief Run method actualize every period.
+ */
void run();
};
--- a/Sensors/HMC5883L.lib Thu Apr 04 06:43:43 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -Sensors/HMC5883L#f4e750710ff6
--- a/Sensors/HMC5883L/HMC5883L.cpp Thu Apr 04 06:43:43 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,421 +0,0 @@
-/**
- * @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
--- a/Sensors/HMC5883L/HMC5883L.h Thu Apr 04 06:43:43 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,303 +0,0 @@
-/**
- * @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
--- a/Sensors/HMC6352.lib Thu Apr 04 06:43:43 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/aberk/code/HMC6352/#cdaa0a63f3cf
--- a/Sensors/Ping.lib Thu Apr 04 06:43:43 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/rosienej/code/Ping/#9c653265fc50
--- a/Sensors/Ping/Ping.cpp Thu Apr 04 06:43:43 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,54 +0,0 @@
-#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
--- a/Sensors/Ping/Ping.h Thu Apr 04 06:43:43 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,92 +0,0 @@
-/* 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
--- a/StateDefines/State.cpp Thu Apr 04 06:43:43 2013 +0000
+++ b/StateDefines/State.cpp Fri Apr 05 10:58:42 2013 +0000
@@ -8,14 +8,20 @@
// LocalFileSystem
LocalFileSystem local("local");
-State::State(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ AnalogIn* battery, float period) : Task(period)
+State::State(state_t* s,
+ RobotControl* robotControl,
+ MaxonESCON* motorControllerLeft,
+ MaxonESCON* motorControllerRight,
+ PinName batteryPin,
+ float period)
+ : Task(period) ,
+ battery(batteryPin)
{
/* get peripherals */
this->s = s;
this->robotControl = robotControl;
this->motorControllerLeft = motorControllerLeft;
this->motorControllerRight = motorControllerRight;
- // this->compass = compass;
this->battery =battery;
this->period = period;
}
@@ -48,10 +54,8 @@
"SetpointAngel[grad]\t" //17
"SetpointVelocitiy[m/s]\t"
"SetpointVelocitiyRotations[rad/s]\t"
- "CompassX-Axis\t" //20
- "CompassY-Axis\t"
- "State\t"
- "distanceToGoal[m]\t" //23
+ "State\t" //20
+ "distanceToGoal[m]\t" //21
"angleToGoal[grad]\t"
"thetaFromTheGoal[grad]\n");
}
@@ -63,7 +67,7 @@
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\t%f\t%f\t%f",
+ "%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d\t%f\t%f\t%f",
s.millis,
s.voltageBattery,
s.leftPulses,
@@ -83,8 +87,6 @@
s.setAngle,
s.setVelocity,
s.setOmega,
- s.compassxAxis,
- s.compassyAxis,
s.state,
s.rho,
s.lamda,
@@ -96,11 +98,6 @@
fprintf(logp, "\n"); // new line
}
-void State::savePlotText(char text[])
-{
- fprintf(logp, text);
-}
-
void State::closePlotFile(void)
{
fclose(logp);
@@ -108,7 +105,7 @@
float State::readBattery()
{
- return battery->read()*BAT_MULTIPLICATOR;
+ return battery.read()*BAT_MULTIPLICATOR;
}
void State::setBatteryBit()
@@ -165,7 +162,6 @@
s->setxAxis = robotControl->getDesiredxPosition();
s->setyAxis = robotControl->getDesiredyPosition();
s->setAngle = robotControl->getDesiredTheta() * 180 / PI;
- // s->compassAngle = compass->getFilteredAngle() * 180 / PI;
s->setVelocity = robotControl->getDesiredSpeed();
s->setOmega = robotControl->getDesiredOmega();
--- a/StateDefines/State.h Thu Apr 04 06:43:43 2013 +0000
+++ b/StateDefines/State.h Fri Apr 05 10:58:42 2013 +0000
@@ -1,24 +1,18 @@
#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 © 2013 HSLU Pren Team #1 Cruising Crêpe
+ * @copyright Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
* All rights reserved.
*
- * @section DESCRIPTION
+ * @brief
*
* State is the main mechanism for communicating current realtime system state to
* the rest of the system for logging etc.
@@ -30,76 +24,68 @@
private:
- state_t* s;
+ state_t* s;
RobotControl* robotControl;
MaxonESCON* motorControllerLeft;
MaxonESCON* motorControllerRight;
- // HMC6352* compass;
- AnalogIn* battery;
+ AnalogIn battery;
Timer timer;
float period;
- //float magout[3];
public:
/**
- * Creates a robot control object and initializes all private state variables.
+ * @brief Creates a robot control object and initializes all private state variables.
* @param s struct to read and write the state
* @param robotControl Object 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 battery Analog Input Battery Voltage input
+ * @param batteryPin mbed pin for analog Input Battery Voltage
* @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,
+ PinName batteryPin,
float period);
/**
- * Destructor of the Object to destroy the Object.
+ * @brief 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.
+ * @brief 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.
+ * @brief Close the File.
*/
void closePlotFile(void);
/**
- * Return the Battery voltage
+ * @brief Return the Battery voltage
* state variables.
* @return Batterie Voltage [V]
*/
float readBattery();
/**
- * Starts the timer from zero.
+ * @brief Starts the timer from zero.
* The timer is for the logging Time.
*/
void startTimerFromZero();
/**
- * Save the new state to a new line.
+ * @brief Save the new state to a new line.
*/
void savePlotFile(state_t s);
+ /**
+ * @brief Run method actualize every period.
+ */
void run();
private:
--- a/StateDefines/defines.h Thu Apr 04 06:43:43 2013 +0000
+++ b/StateDefines/defines.h Fri Apr 05 10:58:42 2013 +0000
@@ -8,9 +8,10 @@
#include "mbed.h"
/**
- * Physical dimensions π
+ * @name Physical dimensions π;
*/
#define PI 3.141592654f
+/*! @} */
/**
* @name maxon motor #339282 EC 45 flat 30W
@@ -18,17 +19,17 @@
*/
/**
-* Number of of pole pairs
+* @brief Number of of pole pairs
*/
#define POLE_PAIRS 8u
/**
-* Gear on the motor 1/11.6f
+* @brief Gear on the motor 1/11.6f
*/
#define GEAR 1/11.6f
/**
-* Pulses per electrical step form the Hallsensor, have 6 steps
+* @brief Pulses per electrical step form the Hallsensor, have 6 steps
*/
#define PULSES_PER_STEP 6u
/*! @} */
@@ -39,24 +40,34 @@
*/
/**
- * Value for the diffrerenz between left an right, given in [m]
+ * @brief Value for the diffrerenz between left an right, given in [m]
*/
#define WHEEL_RADIUS_DIFF 0.0000f
/**
- * radius of the left wheel, given in [m]
+ * @brief Radius of the left wheel, given in [m]
*/
#define WHEEL_RADIUS_LEFT 0.040280f
/**
- * radius of the left wheel, given in [m]
+ * @brief Radius of the left wheel, given in [m]
*/
#define WHEEL_RADIUS_RIGHT (WHEEL_RADIUS_LEFT - WHEEL_RADIUS_DIFF)
/**
- * Distance of the wheel, given in [m] Greater --> turn more
+ * @brief Distance of the wheel, given in [m] Greater --> turn more
*/
#define WHEEL_DISTANCE 0.2000f
+
+/**
+ * @brief Sets the start X-point, given in [m]
+ */
+#define START_X_OFFSET -0.8f
+
+/**
+ * @brief Sets the start Y-point, given in [m]
+ */
+#define START_Y_OFFSET 0.8f
/*! @} */
/**
@@ -65,119 +76,191 @@
*/
/**
- * Bit0 = stop pressed
+ * @brief Bit0 = stop pressed
*/
#define STATE_STOP 1u
/**
- * Bit1 = Undervoltage battery
- */
-#define STATE_UNDER 2u
+ * @brief Bit1 = Undervoltage battery
+ */
+#define STATE_UNDER 2u
/**
- * Bit2 = left ESCON in error state
- */
+ * @brief Bit2 = left ESCON in error state
+ */
#define STATE_LEFT 4u
/**
- * Bit3 = right ESCON in error state
+ * @brief Bit3 = right ESCON in error state
*/
#define STATE_RIGHT 8u
-/*! @} */
+/*! @} */
+
+/**
+ * @name ESCON Constands
+ * @{
+ */
-// ESCON Constands
-#define ESCON_SET_FACTOR 1500.0f // Speed Factor how set in the ESCON
-#define ESCON_GET_FACTOR 1600.4f // Speed Factor how get in the ESCON
+/**
+ * @brief Speed Factor how set in the ESCON Studio
+ */
+#define ESCON_SET_FACTOR 1500.0f
+
+/**
+ * @brief Speed Factor how get in the ESCON Studio
+ */
+#define ESCON_GET_FACTOR 1600.4f
-//Error patch of the drift of Analog input and pwn output
-#define SET_SPEED_PATCH (1+0.00262f) // patch factor of set speed
-#define GET_SPEED_PATCH (1+0.0019f) // patch factor of get speed
+/**
+ * @brief Error patch of the drift of Analog input and pwn output for set speed
+ */
+#define SET_SPEED_PATCH (1+0.00262f)
-// 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]
+/**
+ * @brief Error patch of the drift of Analog input and pwn output for get speed
+ */
+#define GET_SPEED_PATCH (1+0.0019f)
+/*! @} */
-// 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]
+/**
+ * @name position controller
+ * @{
+ */
+
+/**
+ * @brief Main Gain for k1, k2 and k3
+ */
+#define GAIN 0.8f
-// position controller
-#define GAIN 0.8f // Main Gain
+/**
+ * @brief Gain k1 default 1.0f
+ */
#define K1 0.8f * GAIN
-#define K2 3.0f * GAIN // deafult 3.0f
-#define K3 2.0f * GAIN // default 2.0f
-#define MIN_DISTANCE_ERROR -0.005f // min. Distance to switch the position controller off. Because when Distance Error goes to zero the ATAN2 is not define, given in [m]
+
+/**
+ * @brief Gain k2 default 3.0f
+ */
+#define K2 3.0f * GAIN
+
+/**
+ * @brief Gain k3 default 2.0f
+ */
+#define K3 2.0f * GAIN
-// LiPo Batterie
-#define BAT_MULTIPLICATOR 21.633333333f // R2 / (R1 + R2) = 0.153 R2= 10k , R1 = 1.8k 1/0.153 = 6.555 ---> 3.3 * 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
+/**
+ * @brief Min. Distance to switch the position controller off.
+ * Because when Distance Error goes to zero the ATAN2 is not define, given in [m]
+ */
+#define MIN_DISTANCE_ERROR 0.01f
+/*! @} */
-// 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
+/**
+ * @name Batterie control Battery Type: 1SP1P LG-18650
+ * * nominal voltage 3.6V
+ * * 5 batterys ==> 5 * 3.5V = 17.5V
+ * @{
+ */
-// Android Buffer Size for communication
-#define OUTL 100
-#define INBL 100
+/**
+ * @brief Battery Multiplicator for the potential divider.
+ *
+ * R2 / (R1 + R2) = 0.153 R2= 10k , R1 = 1.8k 1/0.153 = 6.555 --> 3.3 * 6.555 = 21.6333333f
+ */
+#define BAT_MULTIPLICATOR 21.633333333f
+
+/**
+ * @brief minium operate voltage, given in [V]
+ */
+#define BAT_MIN 17.5f
+/*! @} */
/**
-* struct state
-* structure containing system sensor data
-**/
+ * @name sampling rate for a Task Object
+ * @{
+ */
+
+/**
+ * @brief 1kHz Rate for Robot Control, given in [s]
+ */
+#define PERIOD_ROBOTCONTROL 0.001f
+
+/**
+ * @brief 1kHz Rate for State Objekt , given in [s]
+ */
+#define PERIOD_STATE 0.001f
+
+/**
+ * @brief 10Hz Rate for the Android communication , given in [s]
+ */
+#define PERIOD_ANDROID 0.1f
+/*! @} */
+
+/**
+ * @name Android Buffer Size for communication
+ * @{
+ */
+
+/**
+ * @brief Buffer Output
+ */
+#define OUTL 100
+
+/**
+ * @brief Buffer Input
+ */
+#define INBL 100
+/*! @} */
+
+/**
+ * @brief struct state
+ * structure containing system sensor data
+ */
typedef struct state {
- /** millis Time [ms]*/
+ /** @brief millis Time [ms]*/
int millis;
- /** Battery voltage [V] */
+ /** @brief Battery voltage [V] */
float voltageBattery;
- /** Number of pulses left */
+ /** @brief Number of pulses left */
int leftPulses;
- /** Number of pulses right */
+ /** @brief Number of pulses right */
int rightPulses;
- /** Velocity left [m/s] */
+ /** @brief Velocity left [m/s] */
float leftVelocity;
- /** Velocity right [m/s] */
+ /** @brief Velocity right [m/s] */
float rightVelocity;
- /** Velocity of the car [m/s] */
+ /** @brief Velocity of the car [m/s] */
float velocity;
- /** Velocity rotation [°/s] */
+ /** @brief Velocity rotation [°/s] */
float omega;
- /** X-Axis from co-ordinate [m] */
+ /** @brief X-Axis from co-ordinate [m] */
float xAxis;
- /** Y-Axis from co-ordinate [m] */
+ /** @brief Y-Axis from co-ordinate [m] */
float yAxis;
- /** X-Axis Error [m] */
+ /** @brief X-Axis Error [m] */
float xAxisError;
- /** X-Axis Error [m] */
+ /** @brief X-Axis Error [m] */
float yAxisError;
- /** Angle Error [°] */
+ /** @brief Angle Error [°] */
float angleError;
- /** Angle from Car [°] */
+ /** @brief Angle from Car [°] */
float angle;
- /** Setpoint X-Axis [m] */
+ /** @brief Setpoint X-Axis [m] */
float setxAxis;
- /** Setpoint Y-Axis [m] */
+ /** @brief Setpoint Y-Axis [m] */
float setyAxis;
- /** Setpoint Angel [°] */
+ /** @brief Setpoint Angel [°] */
float setAngle;
- /** Setpoint velocitiy [m/s] */
+ /** @brief Setpoint velocitiy [m/s] */
float setVelocity;
- /** Setpoint rotation velocitiy [rad/s] */
+ /** @brief Setpoint rotation velocitiy [rad/s] */
float setOmega;
- /** Compass X-Axis */
- float compassxAxis;
- /** Compass Y-Axis */
- float compassyAxis;
- /** State of the Car **/
int state;
- /** distance to Goal */
+ /** @brief distance to Goal */
float rho;
- /** theta to goal */
+ /** @brief theta to goal */
float lamda;
- /** theta from the goal */
+ /** @brief theta from the goal */
float delta;
} state_t;
-
-#endif
\ No newline at end of file
+#endif
--- a/Task/Task.h Thu Apr 04 06:43:43 2013 +0000 +++ b/Task/Task.h Fri Apr 05 10:58:42 2013 +0000 @@ -6,12 +6,10 @@ /** * @author Christian Burri * - * @section LICENSE - * - * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe + * @copyright Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe * All rights reserved. * - * @section DESCRIPTION + * @brief * 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>
--- a/main.cpp Thu Apr 04 06:43:43 2013 +0000
+++ b/main.cpp Fri Apr 05 10:58:42 2013 +0000
@@ -1,105 +1,107 @@
/*! \mainpage Index Page
- *
* @author Christian Burri
+ * @author Arno Galliker
*
- * @section LICENSE
- *
- * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
+ * @copyright Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
* All rights reserved.
*
- * @section DESCRIPTION
+ * @brief
*
* This Programm is for a autonomous robot for the competition
* at the Hochschule Luzern.
* We are one of the 32 teams. In the team #1 is:
- * - Bauernfeind Julia <B>WI</B> <a href="julia.bauernfeind@stud.hslu.ch">julia.bauernfeind@stud.hslu.ch</a>
- * - Büttler Pirmin <B>WI</B> <a href="pirmin.buetler@stud.hslu.ch">pirmin.buetler@stud.hslu.ch</a>
- * - Amberg Reto <B>I</B> <a href="reto.amberg@stud.hslu.ch">reto.amberg@stud.hslu.ch</a>
- * - Galliker Arno <B>I</B> <a href="arno.galliker@stud.hslu.ch">arno.galliker@stud.hslu.ch</a>
- * - Amrein Marcel <B>M</B> <a href="marcel.amrein@stud.hslu.ch">marcel.amrein@stud.hslu.ch</a>
- * - Flühler Ramon <B>M</B> <a href="ramon.fluehler@stud.hslu.ch">ramon.fluehler@stud.hslu.ch</a>
- * - Burri Christian <B>ET</B> <a href="christian.burri@stud.hslu.ch">christian.burri@stud.hslu.ch</a>
+ * - Bauernfeind Julia <B>WI</B> <a href="mailto:julia.bauernfeind@stud.hslu.ch">julia.bauernfeind@stud.hslu.ch</a>
+ * - Büttler Pirmin <B>WI</B> <a href="mailto:pirmin.buetler@stud.hslu.ch">pirmin.buetler@stud.hslu.ch</a>
+ * - Amberg Reto <B>I</B> <a href="mailto:reto.amberg@stud.hslu.ch">reto.amberg@stud.hslu.ch</a>
+ * - Galliker Arno <B>I</B> <a href="mailto:arno.galliker@stud.hslu.ch">arno.galliker@stud.hslu.ch</a>
+ * - Amrein Marcel <B>M</B> <a href="mailto:marcel.amrein@stud.hslu.ch">marcel.amrein@stud.hslu.ch</a>
+ * - Flühler Ramon <B>M</B> <a href="mailto:ramon.fluehler@stud.hslu.ch">ramon.fluehler@stud.hslu.ch</a>
+ * - Burri Christian <B>ET</B> <a href="mailto:christian.burri@stud.hslu.ch">christian.burri@stud.hslu.ch</a>
*
* The postition control is based on polar coordiantes.
* For more information see here: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>
*
+ *
*/
-#include "mbed.h"
-#include "math.h"
+/**
+ * @file main.cpp
+ */
+
#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"
//Android
-//AdkTerm AdkTerm;
+//AdkTerm adkTerm;
+
+/**
+ * @name Hallsensor
+ * @{
+ */
+
+ /**
+ * @brief <code>hallsensorLeft</code> object with pin15, pin17 and pin16
+ */
+Hallsensor hallLeft(p18, p17, p16);
+/**
+ * @brief <code>hallsensorLeft</code> object with pin17, pin28 and pin29
+ */
+Hallsensor hallRight(p27, p28, p29);
+/*! @} */
/**
- * LiPo Batterie for check an undervoltage.
+ * @name Motors and Robot Control
+ * @{
*/
-AnalogIn battery(p15);
-
-// compass
-//HMC5883L compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
-//HMC6352 compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
-
-//Hallsensor
-//hall1, hall2, hall3
-Hallsensor hallLeft(p18, p17, p16);
-//hall1, hall2, hall3
-Hallsensor hallRight(p27, p28, p29);
-
-// Motors
-//enb, ready, pwm, actualSpeed, Hallsensor object
+
+ /**
+ * @brief <code>leftMotor</code> object with pin26, pin25, pin24, pin19 and <code>hallsensorLeft</code> object
+ */
MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft);
-//enb, ready, pwm, actualSpeed, Hallsensor object
+/**
+ * @brief <code>rightMotor</code> object with pin23, pin22, pin21, pin20 and <code>hallsensorRight</code> 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);
+/**
+ * @brief <code>robotControl</code> object with <code>leftMotor</code>, <code>rightMotor</code> and the sampling rate for the run method
+ */
+RobotControl robotControl(&leftMotor, &rightMotor, PERIOD_ROBOTCONTROL);
+/*! @} */
-// PC USB communications
-Serial pc(USBTX, USBRX);
+/**
+ * @name Logging & State
+ * @{
+ */
+
+ /**
+ * @brief Define the struct for the State and the Logging
+ */
+state_t s;
+/**
+ * @brief <code>state</code> object with <code>robotControl</code>, <code>rightMotor</code>, <code>leftMotor</code>, <code>battery</code>, and the sampling rate for the run method
+ */
+State state(&s, &robotControl, &leftMotor, &rightMotor, p15, PERIOD_STATE);
+/*! @} */
-DigitalOut myled(LED1);
-
-
-// LiPo Batterie
-float batterie_voltage;
+// PC USB communications DAs wird danach gelöscht
+//Serial pc(USBTX, USBRX);
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();
-
- state.initPlotFile();
- state.startTimerFromZero();
- state.start();
-
+
+ state.initPlotFile();
+ state.startTimerFromZero();
+ state.start();
+
robotControl.setEnable(false);
wait(0.1);
robotControl.setEnable(true);
wait(0.1);
- //robotControl.setAllToZero(0, 0, PI/2 );
- robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
+ robotControl.setAllToZero(0, 0, PI/2 );
+// robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
robotControl.start();
@@ -108,7 +110,7 @@
// wait(0.1);
//////////////////////////////////////////
- /*
+
robotControl.setDesiredPositionAndAngle(0.0f, 1.0f, PI);
while(!(robotControl.getDistanceError() <= 0.1)) {
state.savePlotFile(s);
@@ -133,7 +135,7 @@
while(!(s.millis >= 32000)) {
state.savePlotFile(s);
};
- */
+
@@ -211,72 +213,77 @@
// Epä Parkour fahrä
-
- robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
- wait(0.1);
+ /*
+ robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
+ wait(0.1);
- robotControl.setDesiredPositionAndAngle(-1.50f, 1.50f, 3*PI/4);
- while(!(robotControl.getDistanceError() <= 0.9)) {
- state.savePlotFile(s);
- };
+ robotControl.setDesiredPositionAndAngle(-1.50f, 1.50f, 3*PI/4);
+ while(!(robotControl.getDistanceError() <= 0.9)) {
+ state.savePlotFile(s);
+ };
- robotControl.setDesiredPositionAndAngle(-1.20f, 2.1f, PI/4);
- while(!(robotControl.getDistanceError() <= 0.7)) {
- state.savePlotFile(s);
- };
+ robotControl.setDesiredPositionAndAngle(-1.20f, 2.1f, PI/4);
+ while(!(robotControl.getDistanceError() <= 0.7)) {
+ state.savePlotFile(s);
+ };
- robotControl.setDesiredPositionAndAngle(-0.75f, 3.0f, PI/2);
- while(!(robotControl.getDistanceError() <= 0.7)) {
- state.savePlotFile(s);
- };
+ robotControl.setDesiredPositionAndAngle(-0.75f, 3.0f, PI/2);
+ while(!(robotControl.getDistanceError() <= 0.7)) {
+ state.savePlotFile(s);
+ };
- robotControl.setDesiredPositionAndAngle(-1.0f, 3.75f, 3*PI/4);
- while(!(robotControl.getDistanceError() <= 0.55)) {
- state.savePlotFile(s);
- };
+ robotControl.setDesiredPositionAndAngle(-1.0f, 3.75f, 3*PI/4);
+ while(!(robotControl.getDistanceError() <= 0.55)) {
+ state.savePlotFile(s);
+ };
- robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI);
- while(!(robotControl.getDistanceError() <= 0.1)) {
- state.savePlotFile(s);
- };
+ robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI);
+ while(!(robotControl.getDistanceError() <= 0.05)) {
+ state.savePlotFile(s);
+ };
+
+ robotControl.setDesiredPositionAndAngle(-2.6f, 3.0f, -PI/2);
+ while(!(robotControl.getDistanceError() <= 1.0)) {
+ state.savePlotFile(s);
+ };
+
+ robotControl.setDesiredPositionAndAngle(-1.74f, 1.30f, -PI/2);
+ while(!(robotControl.getDistanceError() <= 0.1)) {
+ state.savePlotFile(s);
+ };
+ robotControl.setDesiredPositionAndAngle(-1.75f, 0.80f, -PI/2);
+ while(!(s.millis >= 32000)) {
+ state.savePlotFile(s);
+ }
- robotControl.setDesiredPositionAndAngle(-2.4f, 3.0f, -PI/2);
- while(!(robotControl.getDistanceError() <= 1.0)) {
- state.savePlotFile(s);
- };
+ */
+
+
+
+/*
- robotControl.setDesiredPositionAndAngle(-1.74f, 1.30f, -PI/2);
- while(!(robotControl.getDistanceError() <= 0.1)) {
- state.savePlotFile(s);
- };
- robotControl.setDesiredPositionAndAngle(-1.75f, 0.80f, -PI/2);
- while(!(s.millis >= 32000)) {
- state.savePlotFile(s);
+ pc.baud(460800);
+ pc.printf("Welcome to adkTerm\n\n\n\n\n\n\r");
+ pc.printf("here we go... \n");
+ adkTerm.setupDevice();
+ pc.printf("Android Development Kit: start\r\n");
+ USBInit();
+ while (1) {
+ USBLoop();
+
+ pc.printf("Android x: %f ",adkTerm.getx());
+ pc.printf("Android y: %f ",adkTerm.gety());
+ pc.printf("Android t: %f\n",adkTerm.gett());
+ robotControl.setDesiredPositionAndAngle(adkTerm.getx(), adkTerm.gety(), adkTerm.gett());
}
-
- /*
- printf("here we go... \n");
- AdkTerm.setupDevice();
- printf("Android Development Kit: start\r\n");
- USBInit();
- while (!(s.millis >= 60000)) {
- USBLoop();
-
- printf("x: %f y: %f theta: %f", AdkTerm.getx(), AdkTerm.getx(), AdkTerm.getx() )
-
- if( AdkTerm.getx() == 99) {
- break;
- }
- }
- */
-
state.savePlotFile(s);
state.closePlotFile();
state.stop();
robotControl.setEnable(false);
+ */
}