Betago
v1
Dependencies: BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of
CleaningMachine_Betago
by 
palm and chin
Revision 1:45f1573d65a1, committed 2016-03-21
- Comitter:
- palmdotax
- Date:
- Mon Mar 21 20:21:12 2016 +0000
- Parent:
- 0:84f05cd2f197
- Child:
- 2:f873deba2305
- Commit message:
- aaaaa
Changed in this revision
--- a/BEAR_Protocol_Edited.lib Mon Feb 15 17:48:23 2016 +0000 +++ b/BEAR_Protocol_Edited.lib Mon Mar 21 20:21:12 2016 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/teams/BEaR-lab/code/BEAR_Protocol/#6296cb35f853 +http://developer.mbed.org/teams/BEaR-lab/code/BEAR_Protocol/#24d951efed53
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motion_EEPROM_Address.h Mon Mar 21 20:21:12 2016 +0000 @@ -0,0 +1,25 @@ +#ifndef __MOTION__EEPROM__ADDRESS__H_ +#define __MOTION__EEPROM__ADDRESS__H_ + +#define ADDRESS_ID 0x00 +#define ADDRESS_UPPER_KP 0x04 +#define ADDRESS_UPPER_KI 0x08 +#define ADDRESS_UPPER_KD 0x0c +#define ADDRESS_LOWER_KP 0x10 +#define ADDRESS_LOWER_KI 0x14 +#define ADDRESS_LOWER_KD 0x18 +#define ADDRESS_UP_MARGIN 0x1c +#define ADDRESS_LOW_MARGIN 0x20 +#define ADDRESS_ANGLE_RANGE_UP 0x24 //reserved 2 bytes +#define ADDRESS_ANGLE_RANGE_LOW 0x2c //reserved 2 bytes +#define ADDRESS_UP_LINK_LENGTH 0x34 +#define ADDRESS_LOW_LINK_LENGTH 0x38 +#define ADDRESS_WHEELPOS 0x3c +#define ADDRESS_HEIGHT 0x40 +#define ADDRESS_OFFSET 0x44 //reserved 2 bytes +#define ADDRESS_BODY_WIDTH 0x4c +#define ADDRESS_MAG_DATA 0x50 //reserved 6 bytes + + +#endif +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/UNTRASONIC.cpp Mon Mar 21 20:21:12 2016 +0000
@@ -0,0 +1,117 @@
+#include "mbed.h"
+#include "MaxSonar.h"
+#include "UNTRASONIC.h"
+MaxSonar *range1;
+MaxSonar *range2;
+MaxSonar *range3;
+MaxSonar *range4;
+MaxSonar *range5;
+MaxSonar *range6;
+MaxSonar *range7;
+MaxSonar *range8;
+MaxSonar *range9;
+MaxSonar *range10;
+sensor::sensor()
+{
+ sen_1=0;
+ sen_2=0;
+ sen_3=0;
+ sen_4=0;
+ sen_5=0;
+ sen_6=0;
+ sen_7=0;
+ sen_8=0;
+ sen_9=0;
+ sen_10=0;
+
+
+}
+void sensor::get_sen()
+{
+ range1->triggerRead();
+ sen_1 = range1->read();
+ //ส่งค่sensor
+ range2->triggerRead();
+ sen_2 = range1->read();
+ //ส่งค่sensor
+ range3->triggerRead();
+ sen_3 = range1->read();
+ //ส่งค่sensor
+ range4->triggerRead();
+ sen_4 = range1->read();
+ //ส่งค่sensor
+ range5->triggerRead();
+ sen_5 = range1->read();
+ //ส่งค่sensor
+ range6->triggerRead();
+ sen_6 = range1->read();
+ //ส่งค่sensor
+ range7->triggerRead();
+ sen_7 = range1->read();
+ //ส่งค่sensor
+ range8->triggerRead();
+ sen_8 = range1->read();
+ //ส่งค่sensor
+ range9->triggerRead();
+ sen_9 = range1->read();
+ //ส่งค่sensor
+ range10->triggerRead();
+ sen_10 = range1->read();
+ //ส่งค่sensor
+}
+void sensor::inti()
+{
+ //MaxSonar *range1;
+ range1 = new MaxSonar(MS_LV, MS_ANALOG, PC_7, PA_0);
+ range1->setVoltage(3.3);
+ range1->setUnits(MS_CM);
+
+ //MaxSonar *range2;
+ range2 = new MaxSonar(MS_LV, MS_ANALOG, PC_7, PA_1);
+ range2->setVoltage(3.3);
+ range2->setUnits(MS_CM);
+
+ // MaxSonar *range3;
+ range3 = new MaxSonar(MS_LV, MS_ANALOG, PC_7, PA_4);
+ range3->setVoltage(3.3);
+ range3->setUnits(MS_CM);
+
+ //MaxSonar *range4;
+ range4 = new MaxSonar(MS_LV, MS_ANALOG, PC_7, PB_0);
+ range4->setVoltage(3.3);
+ range4->setUnits(MS_CM);
+
+ //MaxSonar *range5;
+ range5 = new MaxSonar(MS_LV, MS_ANALOG, PC_7, PC_1);
+ range5->setVoltage(3.3);
+ range5->setUnits(MS_CM);
+
+ // MaxSonar *range6;
+ range6 = new MaxSonar(MS_LV, MS_ANALOG, PC_7, PC_0);
+ range6->setVoltage(3.3);
+ range6->setUnits(MS_CM);
+
+ //MaxSonar *range7;
+ range7 = new MaxSonar(MS_LV, MS_ANALOG, PC_7, PC_2);
+ range7->setVoltage(3.3);
+ range7->setUnits(MS_CM);
+
+ // MaxSonar *range8;
+ range8 = new MaxSonar(MS_LV, MS_ANALOG, PC_7, PC_3);
+ range8->setVoltage(3.3);
+ range8->setUnits(MS_CM);
+
+ // MaxSonar *range9;
+ range9 = new MaxSonar(MS_LV, MS_ANALOG, PC_7, PC_4);
+ range9->setVoltage(3.3);
+ range9->setUnits(MS_CM);
+
+ // MaxSonar *range10;
+ range10 = new MaxSonar(MS_LV, MS_ANALOG, PC_7, PC_5);
+ range10->setVoltage(3.3);
+ range10->setUnits(MS_CM);
+}
+void sensor::readbat()
+{
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/UNTRASONIC.h Mon Mar 21 20:21:12 2016 +0000
@@ -0,0 +1,13 @@
+#ifndef UNTRASONIC_H
+#define UNTRASONIC_H
+class sensor
+{
+ private:float sen_1,sen_2,sen_3,sen_4,sen_5,sen_6,sen_7,sen_8,sen_9,sen_10;
+ public : void get_sen();
+ void get_motor();
+ void inti();
+ void readbat();
+ sensor();
+
+};
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/eeprom/eeprom.cpp Mon Mar 21 20:21:12 2016 +0000
@@ -0,0 +1,816 @@
+/***********************************************************
+Author: Bernard Borredon
+Date: 27 december 2011
+Version: 1.0
+************************************************************/
+#include "mbed.h"
+#include "eeprom.h"
+
+#define BIT_SET(x,n) (x=x | (0x01<<n))
+#define BIT_TEST(x,n) (x & (0x01<<n))
+#define BIT_CLEAR(x,n) (x=x & ~(0x01<<n))
+
+EEPROM::EEPROM(PinName sda, PinName scl, uint8_t address, TypeEeprom type) : _i2c(sda, scl)
+{
+
+ _errnum = EEPROM_NoError;
+ _type = type;
+
+ // Check address range
+ _address = address;
+ switch(type) {
+ case T24C01 :
+ case T24C02 :
+ if(address > 7) {
+ _errnum = EEPROM_BadAddress;
+ }
+ _address = _address << 1;
+ _page_write = 8;
+ _page_number = 1;
+ break;
+ case T24C04 :
+ if(address > 7) {
+ _errnum = EEPROM_BadAddress;
+ }
+ _address = (_address & 0xFE) << 1;
+ _page_write = 16;
+ _page_number = 2;
+ break;
+ case T24C08 :
+ if(address > 7) {
+ _errnum = EEPROM_BadAddress;
+ }
+ _address = (_address & 0xFC) << 1;
+ _page_write = 16;
+ _page_number = 4;
+ break;
+ case T24C16 :
+ _address = 0;
+ _page_write = 16;
+ _page_number = 8;
+ break;
+ case T24C32 :
+ case T24C64 :
+ if(address > 7) {
+ _errnum = EEPROM_BadAddress;
+ }
+ _address = _address << 1;
+ _page_write = 32;
+ _page_number = 1;
+ break;
+ case T24C128 :
+ case T24C256 :
+ if(address > 3) {
+ _errnum = EEPROM_BadAddress;
+ }
+ _address = _address << 1;
+ _page_write = 64;
+ _page_number = 1;
+ break;
+ case T24C512 :
+ if(address > 3) {
+ _errnum = EEPROM_BadAddress;
+ }
+ _address = _address << 1;
+ _page_write = 128;
+ _page_number = 1;
+ break;
+ case T24C1024 :
+ if(address > 7) {
+ _errnum = EEPROM_BadAddress;
+ }
+ _address = (_address & 0xFE) << 1;
+ _page_write = 128;
+ _page_number = 2;
+ break;
+ case T24C1025 :
+ if(address > 3) {
+ _errnum = EEPROM_BadAddress;
+ }
+ _address = _address << 1;
+ _page_write = 128;
+ _page_number = 2;
+ break;
+ }
+
+ // Size in bytes
+ _size = _type;
+ if(_type == T24C1025)
+ _size = T24C1024;
+
+ // Set I2C frequency
+ _i2c.frequency(100000);
+}
+
+void EEPROM::write(uint16_t address, int8_t data)
+{
+ uint8_t page;
+ uint8_t addr;
+ uint8_t cmd[3];
+ int len;
+ int ack;
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ // Compute page number
+ page = 0;
+ if(_type < T24C32)
+ page = (uint8_t) (address / 256);
+
+ // Device address
+ addr = EEPROM_Address | _address | (page << 1);
+
+ if(_type < T24C32) {
+ len = 2;
+
+ // Word address
+ cmd[0] = (uint8_t) (address - page * 256);
+
+ // Data
+ cmd[1] = (uint8_t) data;
+ } else {
+ len = 3;
+
+ // First word address (MSB)
+ cmd[0] = (uint8_t) (address >> 8);
+
+ // Second word address (LSB)
+ cmd[1] = (uint8_t) address;
+
+ // Data
+ cmd[2] = (uint8_t) data;
+ }
+
+// printf("len %d address %02x cmd[0] %02x cmd[1] %02x cmd[2] %02x\n",len,addr,cmd[0],cmd[1],cmd[2]);
+
+ ack = _i2c.write((int)addr,(char *)cmd,len);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+ // Wait end of write
+ ready();
+
+}
+
+void EEPROM::write(uint16_t address, int8_t data[], uint16_t length)
+{
+ uint8_t page;
+ uint8_t addr;
+ uint8_t blocs,remain;
+ uint8_t fpart,lpart;
+ uint8_t i,j,ind;
+ uint8_t cmd[129];
+ int ack;
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ // Check length
+ if(!checkAddress(address + length - 1)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ // Compute blocs numbers
+ blocs = length / _page_write;
+
+ // Compute remaining bytes
+ remain = length - blocs * _page_write;
+
+ for(i = 0; i < blocs; i++) {
+ // Compute page number
+ page = 0;
+ if(_type < T24C32)
+ page = (uint8_t) (address / 256);
+
+ // Device address
+ addr = EEPROM_Address | _address | (page << 1);
+
+ if(_type < T24C32) {
+ // Word address
+ cmd[0] = (uint8_t) (address - page * 256);
+
+ if((uint8_t) ((address + _page_write) / 256) == page) { // Data fit in the same page
+ // Add data
+ for(j = 0; j < _page_write; j++)
+ cmd[j + 1] = (uint8_t) data[i * _page_write + j];
+
+ // Write data
+ ack = _i2c.write((int)addr,(char *)cmd,_page_write + 1);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+ // Wait end of write
+ ready();
+
+ // Increment address
+ address += _page_write;
+ } else { // Data on 2 pages. We must split the write
+ // Number of bytes in current page
+ fpart = (page + 1) * 256 - address;
+
+ // Add data for current page
+ for(j = 0; j < fpart; j++)
+ cmd[j + 1] = (uint8_t) data[i * _page_write + j];
+
+ // Write data for current page
+ ack = _i2c.write((int)addr,(char *)cmd,fpart + 1);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+ // Wait end of write
+ ready();
+
+ // Increment address
+ address += fpart;
+
+ if(page < _page_number - 1) {
+ // Increment page
+ page++;
+
+ // Device address
+ addr = EEPROM_Address | _address | (page << 1);
+
+ // Word address
+ cmd[0] = (uint8_t) (address - page * 256);
+
+ // Data index
+ ind = i * _page_write + fpart;
+
+ // Number of bytes in next page
+ lpart = _page_write - fpart;
+
+ // Add data for next page
+ for(j = 0; j < lpart; j++)
+ cmd[j + 1] = (uint8_t) data[ind + j];
+
+ // Write data for next page
+ ack = _i2c.write((int)addr,(char *)cmd,lpart + 1);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+ // Wait end of write
+ ready();
+
+ // Increment address
+ address += lpart;
+ }
+ }
+ } else {
+ // First word address (MSB)
+ cmd[0] = (uint8_t) (address >> 8);
+
+ // Second word address (LSB)
+ cmd[1] = (uint8_t) address;
+
+ // Add data
+ for(j = 0; j < _page_write; j++)
+ cmd[j + 2] = (uint8_t) data[i * _page_write + j];
+
+ // Write data
+ ack = _i2c.write((int)addr,(char *)cmd,_page_write + 2);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+ // Wait end of write
+ ready();
+
+ // Increment address
+ address += _page_write;
+ }
+ }
+
+ // Compute page number
+ page = 0;
+ if(_type < T24C32)
+ page = (uint8_t) (address / 256);
+
+ // Device address
+ addr = EEPROM_Address | _address | (page << 1);
+
+ if(_type < T24C32) {
+ // Word address
+ cmd[0] = (uint8_t) (address - page * 256);
+
+ if((uint8_t) ((address + remain) / 256) == page) { // Data fit in the same page
+ // Add data for the current page
+ for(j = 0; j < remain; j++)
+ cmd[j + 1] = (uint8_t) data[blocs * _page_write + j];
+
+ // Write data for the current page
+ ack = _i2c.write((int)addr,(char *)cmd,remain + 1);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+ // Wait end of write
+ ready();
+ } else { // Data on 2 pages. We must split the write
+ // Number of bytes in current page
+ fpart = (page + 1) * 256 - address;
+
+ // Add data for current page
+ for(j = 0; j < fpart; j++)
+ cmd[j + 1] = (uint8_t) data[blocs * _page_write + j];
+
+ // Write data for current page
+ ack = _i2c.write((int)addr,(char *)cmd,fpart + 1);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+ // Wait end of write
+ ready();
+
+ // Increment address
+ address += fpart;
+
+ if(page < _page_number - 1) {
+ // Increment page
+ page++;
+
+ // Device address
+ addr = EEPROM_Address | _address | (page << 1);
+
+ // Word address
+ cmd[0] = (uint8_t) (address - page * 256);
+
+ // Data index
+ ind = blocs * _page_write + fpart;
+
+ // Number of bytes in next page
+ lpart = remain - fpart;
+
+ // Add data for next page
+ for(j = 0; j < lpart; j++)
+ cmd[j + 1] = (uint8_t) data[ind + j];
+
+ // Write data for next page
+ ack = _i2c.write((int)addr,(char *)cmd,lpart + 1);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+ // Wait end of write
+ ready();
+ }
+ }
+ } else {
+ // Fist word address (MSB)
+ cmd[0] = (uint8_t) (address >> 8);
+
+ // Second word address (LSB)
+ cmd[1] = (uint8_t) address;
+
+ // Add data for the current page
+ for(j = 0; j < remain; j++)
+ cmd[j + 2] = (uint8_t) data[blocs * _page_write + j];
+
+ // Write data for the current page
+ ack = _i2c.write((int)addr,(char *)cmd,remain + 2);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+ // Wait end of write
+ ready();
+ }
+
+}
+
+void EEPROM::write(uint16_t address, int16_t data)
+{
+ int8_t cmd[2];
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address + 1)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ memcpy(cmd,&data,2);
+
+ write(address,cmd,2);
+
+}
+
+void EEPROM::write(uint16_t address, int32_t data)
+{
+ int8_t cmd[4];
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address + 3)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ memcpy(cmd,&data,4);
+
+ write(address,cmd,4);
+
+}
+
+void EEPROM::write(uint16_t address, float data)
+{
+ int8_t cmd[4];
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address + 3)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ memcpy(cmd,&data,4);
+
+ write(address,cmd,4);
+
+}
+
+void EEPROM::write(uint16_t address, void *data, uint16_t size)
+{
+ int8_t *cmd = NULL;
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address + size - 1)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ cmd = (int8_t *)malloc(size);
+ if(cmd == NULL) {
+ _errnum = EEPROM_MallocError;
+ return;
+ }
+
+ memcpy(cmd,data,size);
+
+ write(address,cmd,size);
+
+ free(cmd);
+
+}
+
+void EEPROM::read(uint16_t address, int8_t& data)
+{
+ uint8_t page;
+ uint8_t addr;
+ uint8_t cmd[2];
+ uint8_t len;
+ int ack;
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ // Compute page number
+ page = 0;
+ if(_type < T24C32)
+ page = (uint8_t) (address / 256);
+
+ // Device address
+ addr = EEPROM_Address | _address | (page << 1);
+
+ if(_type < T24C32) {
+ len = 1;
+
+ // Word address
+ cmd[0] = (uint8_t) (address - page * 256);
+ } else {
+ len = 2;
+
+ // First word address (MSB)
+ cmd[0] = (uint8_t) (address >> 8);
+
+ // Second word address (LSB)
+ cmd[1] = (uint8_t)address;
+ }
+
+ // Write command
+ ack = _i2c.write((int)addr,(char *)cmd,len,true);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+ // Read data
+ ack = _i2c.read((int)addr,(char *)&data,sizeof(data));
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+}
+
+void EEPROM::read(uint16_t address, int8_t *data, uint16_t size)
+{
+ uint8_t page;
+ uint8_t addr;
+ uint8_t cmd[2];
+ uint8_t len;
+ int ack;
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ // Check size
+ if(!checkAddress(address + size - 1)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ // Compute page number
+ page = 0;
+ if(_type < T24C32)
+ page = (uint8_t) (address / 256);
+
+ // Device address
+ addr = EEPROM_Address | _address | (page << 1);
+
+ if(_type < T24C32) {
+ len = 1;
+
+ // Word address
+ cmd[0] = (uint8_t) (address - page * 256);
+ } else {
+ len = 2;
+
+ // First word address (MSB)
+ cmd[0] = (uint8_t) (address >> 8);
+
+ // Second word address (LSB)
+ cmd[1] = (uint8_t) address;
+ }
+
+ // Write command
+ ack = _i2c.write((int)addr,(char *)cmd,len,true);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+ // Sequential read
+ ack = _i2c.read((int)addr,(char *)data,size);
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+}
+
+void EEPROM::read(int8_t& data)
+{
+ uint8_t addr;
+ int ack;
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Device address
+ addr = EEPROM_Address | _address;
+
+ // Read data
+ ack = _i2c.read((int)addr,(char *)&data,sizeof(data));
+ if(ack != 0) {
+ _errnum = EEPROM_I2cError;
+ return;
+ }
+
+}
+
+void EEPROM::read(uint16_t address, int16_t& data)
+{
+ int8_t cmd[2];
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address + 1)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ read(address,cmd,2);
+
+ memcpy(&data,cmd,2);
+
+}
+
+void EEPROM::read(uint16_t address, int32_t& data)
+{
+ int8_t cmd[4];
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address + 3)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ read(address,cmd,4);
+
+ memcpy(&data,cmd,4);
+
+}
+
+void EEPROM::read(uint16_t address, float& data)
+{
+ int8_t cmd[4];
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address + 3)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ read(address,cmd,4);
+
+ memcpy(&data,cmd,4);
+
+}
+
+void EEPROM::read(uint16_t address, void *data, uint16_t size)
+{
+ int8_t *cmd = NULL;
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Check address
+ if(!checkAddress(address + size - 1)) {
+ _errnum = EEPROM_OutOfRange;
+ return;
+ }
+
+ cmd = (int8_t *)malloc(size);
+ if(cmd == NULL) {
+ _errnum = EEPROM_MallocError;
+ return;
+ }
+
+ read(address,cmd,size);
+
+ memcpy(data,cmd,size);
+
+ free(cmd);
+
+}
+
+void EEPROM::ready(void)
+{
+ int ack;
+ uint8_t addr;
+ uint8_t cmd;
+
+ // Check error
+ if(_errnum)
+ return;
+
+ // Device address
+ addr = EEPROM_Address | _address;
+
+ cmd = 0;
+ /*
+ // Wait end of write
+ do {
+ ack = _i2c.write((int)addr,(char *)cmd,0);
+ } while(ack != 0);
+ */
+ wait_ms(5);
+}
+
+uint32_t EEPROM::getSize(void)
+{
+ return(_size);
+}
+
+uint8_t EEPROM::getError(void)
+{
+ return(_errnum);
+}
+
+bool EEPROM::checkAddress(uint16_t address)
+{
+ bool ret = true;
+
+ switch(_type) {
+ case T24C01 :
+ if(address >= T24C01)
+ ret = false;
+ break;
+ case T24C02 :
+ if(address >= T24C02)
+ ret = false;
+ break;
+ case T24C04 :
+ if(address >= T24C04)
+ ret = false;
+ break;
+ case T24C08 :
+ if(address >= T24C08)
+ ret = false;
+ break;
+ case T24C16 :
+ if(address >= T24C16)
+ ret = false;
+ break;
+ case T24C32 :
+ if(address >= T24C32)
+ ret = false;
+ break;
+ case T24C64 :
+ if(address >= T24C64)
+ ret = false;
+ break;
+ case T24C128 :
+ if(address >= T24C128)
+ ret = false;
+ break;
+ case T24C256 :
+ if(address >= T24C256)
+ ret = false;
+ break;
+ case T24C512 :
+ if(address >= T24C512)
+ ret = false;
+ break;
+ case T24C1024 :
+ if(address >= T24C1024)
+ ret = false;
+ break;
+ case T24C1025 :
+ if(address >= T24C1025 - 1)
+ ret = false;
+ break;
+ }
+
+ return(ret);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/eeprom/eeprom.h Mon Mar 21 20:21:12 2016 +0000
@@ -0,0 +1,195 @@
+#ifndef __EEPROM__H_
+#define __EEPROM__H_
+
+// Includes
+#include <string>
+#include "mbed.h"
+
+// Defines
+#define EEPROM_Address 0xA0
+
+#define EEPROM_NoError 0x00
+#define EEPROM_BadAddress 0x01
+#define EEPROM_I2cError 0x02
+#define EEPROM_ParamError 0x03
+#define EEPROM_OutOfRange 0x04
+#define EEPROM_MallocError 0x05
+
+#define EEPROM_MaxError 6
+
+static std::string _ErrorMessageEEPROM[EEPROM_MaxError] = { "",
+ "Bad chip address",
+ "I2C error (nack)",
+ "Invalid parameter",
+ "Data address out of range",
+ "Memory allocation error"};
+
+// Class
+class EEPROM
+{
+public:
+ enum TypeEeprom {T24C01=128,T24C02=256,T24C04=512,T24C08=1024,T24C16=2048,
+ T24C32=4096,T24C64=8192,T24C128=16384,T24C256=32768,
+ T24C512=65536,T24C1024=131072,T24C1025=131073
+ } Type;
+
+ /*
+ * Constructor, initialize the eeprom on i2c interface.
+ * @param sda : sda i2c pin (PinName)
+ * @param scl : scl i2c pin (PinName)
+ * @param address : eeprom address, according to eeprom type (uint8_t)
+ * @param type : eeprom type (TypeEeprom)
+ * @return none
+ */
+ EEPROM(PinName sda, PinName scl, uint8_t address, TypeEeprom type=T24C64);
+
+ /*
+ * Random read byte
+ * @param address : start address (uint16_t)
+ * @param data : byte to read (int8_t&)
+ * @return none
+ */
+ void read(uint16_t address, int8_t& data);
+
+ /*
+ * Random read short
+ * @param address : start address (uint16_t)
+ * @param data : short to read (int16_t&)
+ * @return none
+ */
+ void read(uint16_t address, int16_t& data);
+
+ /*
+ * Random read long
+ * @param address : start address (uint16_t)
+ * @param data : long to read (int32_t&)
+ * @return none
+ */
+ void read(uint16_t address, int32_t& data);
+
+ /*
+ * Random read float
+ * @param address : start address (uint16_t)
+ * @param data : float to read (float&)
+ * @return none
+ */
+ void read(uint16_t address, float& data);
+
+ /*
+ * Random read anything
+ * @param address : start address (uint16_t)
+ * @param data : data to read (void *)
+ * @param size : number of bytes to read (uint16_t)
+ * @return none
+ */
+ void read(uint16_t address, void *data, uint16_t size);
+
+ /*
+ * Current address read byte
+ * @param data : byte to read (int8_t&)
+ * @return none
+ */
+ void read(int8_t& data);
+
+ /*
+ * Sequential read byte
+ * @param address : start address (uint16_t)
+ * @param data : bytes array to read (int8_t[]&)
+ * @param size : number of bytes to read (uint16_t)
+ * @return none
+ */
+ void read(uint16_t address, int8_t *data, uint16_t size);
+
+ /*
+ * Write byte
+ * @param address : start address (uint16_t)
+ * @param data : byte to write (int8_t)
+ * @return none
+ */
+ void write(uint16_t address, int8_t data);
+
+ /*
+ * Write short
+ * @param address : start address (uint16_t)
+ * @param data : short to write (int16_t)
+ * @return none
+ */
+ void write(uint16_t address, int16_t data);
+
+ /*
+ * Write long
+ * @param address : start address (uint16_t)
+ * @param data : long to write (int32_t)
+ * @return none
+ */
+ void write(uint16_t address, int32_t data);
+
+ /*
+ * Write float
+ * @param address : start address (uint16_t)
+ * @param data : float to write (float)
+ * @return error number if > 0 (uint8_t)
+ */
+ void write(uint16_t address, float data);
+
+ /*
+ * Write anything
+ * @param address : start address (uint16_t)
+ * @param data : data to write (void *)
+ * @param size : number of bytes to read (uint16_t)
+ * @return none
+ */
+ void write(uint16_t address, void *data, uint16_t size);
+
+ /*
+ * Write page
+ * @param address : start address (uint16_t)
+ * @param data : bytes array to write (int8_t[])
+ * @param size : number of bytes to write (uint16_t)
+ * @return none
+ */
+ void write(uint16_t address, int8_t data[], uint16_t size);
+
+ /*
+ * Wait eeprom ready
+ * @param : none
+ * @return none
+ */
+ void ready(void);
+
+ /*
+ * Get eeprom size in bytes
+ * @param : none
+ * @return size in bytes (uint16_t)
+ */
+ uint32_t getSize(void);
+
+ /*
+ * Get the current error number (EEPROM_NoError if no error)
+ * @param : none
+ * @return current error number (uint8_t)
+ */
+ uint8_t getError(void);
+
+ /*
+ * Get current error message
+ * @param : none
+ * @return current error message(std::string)
+ */
+ std::string getErrorMessage(void) {
+ return(_ErrorMessageEEPROM[_errnum]);
+ }
+
+//---------- local variables ----------
+private:
+ I2C _i2c; // Local i2c communication interface instance
+ int _address; // Local ds1621 i2c address
+ uint8_t _errnum; // Error number
+ TypeEeprom _type; // EEPROM type
+ uint8_t _page_write; // Page write size
+ uint8_t _page_number; // Number of page
+ uint32_t _size; // Size in bytes
+ bool checkAddress(uint16_t address); // Check address range
+//-------------------------------------
+};
+#endif
--- a/main.cpp Mon Feb 15 17:48:23 2016 +0000
+++ b/main.cpp Mon Mar 21 20:21:12 2016 +0000
@@ -1,63 +1,750 @@
-#include "Debug.h"
-#include "UI.h"
+//*****************************************************/
+// Include //
+#include "mbed.h"
#include "pinconfig.h"
-#include "BEAR_Protocol.h"
+#include "PID.h"
+//#include "Motor.h"
+#include "eeprom.h"
+#include "Receiver.h"
+#include "Motion_EEPROM_Address.h"
+#include "move.h"
+#include "UNTRASONIC.h"
+
+//#include "pidcontrol.h"
+
+#define EEPROM_DELAY 2
+DigitalOut rs485_dirc1(RS485_DIRC);
+//#define DEBUG_UP
+//#define DEBUG_LOW
+
+InterruptIn encoderA_d(PB_12);
+DigitalIn encoderB_d(PB_13);
+InterruptIn encoderA_1(PB_1);
+DigitalIn encoderB_1(PB_2);
+InterruptIn encoderA_2(PB_14);
+DigitalIn encoderB_2(PB_15);
+DigitalOut rs485_dirc1(RS485_DIRC);
+Timer timerStart;
+Timeout time_getsensor;
+Timeout time_distance;
+Timeout shutdown;
+move m1;
+//*****************************************************/
+//--PID parameter--
+//-Upper-//
+float U_Kc=0.2;
+float U_Ki_gain=0.0;
+float U_Kd_gain=0.0;
+float U_Ti=0.0;
+float U_Td=0.0;
+float U_Ki=U_Kc*U_Ti;
+float U_Kd=U_Kc*U_Td;
+//-lower-//
+float L_Kc=0.2;
+float L_Ki_gain=0.0;
+float L_Kd_gain=0.0;
+float L_Ti=0.0;
+float L_Td=0.0;
+float L_Ki=L_Kc*L_Ti;
+float L_Kd=L_Kc*L_Td;
+//*****************************************************/
+// Global //
+//timer
+ int timer_now=0,timer_later=0;
+ int times=0,timer_buffer=0;
+
+ //encoder
+ int Encoderpos = 0;
+ int real_d=0;
+ float valocity1 =0,valocity2 =0,pulse_1=0,pulse_2=0,count=0,r=0.125,velocityreal=0,pulse_d=0,Z_d=0;
+//pid
+
+double setp1=0,setp2=0;
+float outPID =0;
+float VRmax,VLmax,VR,VL,KP_LEFT,KI_LEFT,KD_LEFT,KP_RIGHT,KI_RIGHT ,KD_RIGHT ;
+PID P1(KP_LEFT,KI_LEFT,KD_LEFT,0.1);
+PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1);
+//Ticker Recieve;
+//-- Communication --
+COMMUNICATION *com1;
+Serial PC(SERIAL_TX,SERIAL_RX);
+Bear_Receiver com(SERIAL_TX,SERIAL_RX,115200);
+int16_t MY_ID = 0x00;
+//-- Memorry --
+EEPROM memory(PB_4,PA_8,0);
+
+//-- encoder --
+
+//-- Motor --
+//int dir;
+//Motor Upper(PWM_LU,A_LU,B_LU);
+//Motor Lower(PWM_LL,A_LL,B_LL);
+//-- PID --
+//int Upper_SetPoint=20;
+//int Lower_SetPoint=8;
+//PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
+//PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
+
+//PID Up_PID(U_Kc, U_Ti, U_Td);//Kp,Ki,Kd,Rate
+//PID Low_PID(L_Kc, L_Ti, L_Td);
+
+//*****************************************************/
+//void Start_Up();
+void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
+
+//Timer counterUP;
+//Timer counterLOW;
+
+DigitalOut myled(LED1);
+
-DigitalIn button(USER_BUTTON);
-Serial pc(SERIAL_TX,SERIAL_RX);
+void Rx_interrupt()
+{
+ //s1.get_motor();รับค่ามอเตอร์
+ RC();
+ timer_later= timer_now;
+
+}
+void EncoderA_1()//ซ้าย
+{ if(encoderB_1==0)
+ { Encoderpos = Encoderpos + 1;}
+ else
+ { Encoderpos = Encoderpos -1;}
+ pulse_1+=1;
+ //Encoderpos = Encoderpos + 1;
+ //valocity+=1;
+ //pc.printf("%d \n",Encoderpos);
+ //pc.printf("pulse=%f \n",pulse);
+ //if(pulse==128)
+ //{count+=1;pulse=0; pc.printf("count=%f \n",count);}
+}
+ void EncoderA_2()//ขวา
+{
+ if(encoderB_2==0)
+ { Encoderpos = Encoderpos + 1;}
+ else
+ { Encoderpos = Encoderpos -1;}
+ pulse_2+=1;
+ //pc.printf("%d",Encoderpos);
+}
+void EncoderA_D()
+{
+ if(encoderB_d==0)
+ { Encoderpos = Encoderpos + 1;}
+ else
+ { Encoderpos = Encoderpos -1;}
+ pulse_d+=1;
+ if(pulse_d==128)
+ {
+ Z_d+=1;
+ pulse_d=0;
+ }
+ // pc.printf("%d",Encoderpos);
+}
+void getvelo1()//จาก encoder
+{
+ valocity1=pulse_1*((2*3.14*r)/128);
+ pc.printf("valocity=%f \n",valocity1);
+ count=0;
+ timerStart.reset();
+}
+void getvelo2()
+{
+ valocity2=pulse_2*((2*3.14*r)/128);
+ pc.printf("valocity=%f \n",valocity2);
+ count=0;
+ timerStart.reset();
+}
+void get_d()//ระยะทาง
+{
+ real_d=Z_d*(2*3.14*r);
+ //ส่งข้อมูล
+
+}
+double map(double x, double in_min, double in_max, double out_min, double out_max)
+{
+ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+
+}
+void PID_m1()//left
+{
+ setp1=map(1.0,0.0,1.094,0.0,1.0);
+ P1.setSetPoint(setp1);
+ times=timerStart.read();
+ if(times==1)// m/s
+ {
+ getvelo1();
+ //pc.printf("TIME \n");
+ times=0;
+ pulse_1=0;
+ }
+ P1.setProcessValue(valocity1);
+ outPID=P1.compute();
+ //pc.printf("outPID=%f \n",outPID);
+ m1.movespeed_1(setp1,outPID);
+}
+void PID_m2()//right
+{
+ setp2=map(1.0,0.0,1.094,0.0,1.0);
+ P2.setSetPoint(setp2);
+ times=timerStart.read();
+ if(times==1)// m/s
+ {
+ getvelo2();
+ //pc.printf("TIME \n");
+ times=0;
+ pulse_2=0;
+ }
+ P2.setProcessValue(valocity2);
+ outPID=P2.compute();
+ //pc.printf("outPID=%f \n",outPID);
+ m1.movespeed_2(1,setp2,outPID);
+}
+/*
+void Read_Encoder(PinName Encoder)
+{
+ ENC.format(8,0);
+ ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
+
+ if(Encoder == EncoderA) {
+ EncA = 0;
+ } else {
+ EncB = 0;
+ }
+ ENC.write(0x41);
+ ENC.write(0x09);
+ data = ENC.write(0x00);
+ if(Encoder == EncoderA) {
+ EncA = 1;
+ } else {
+ EncB = 1;
+ }
-void DebugMode();
+}
+//****************************************************
+int Get_EnValue(int Val)
+{
+ int i = 0;
+ static unsigned char codes[] = {
+ 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
+ 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
+ 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
+ 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
+ 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
+ 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
+ 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
+ 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
+ };
+ if ( MY_ID == 0x01 ) { //when it was left side
+ while(Val != codes[i]) {
+ i++;
+ }
+ }
+ if ( MY_ID == 0x02 ) { //when it was right side
+ while(Val != codes[127-i]) {
+ i++;
+ }
+ }
+ return i;
+
+}
+//****************************************************
+void SET_UpperPID()
+{
+ Upper.period(0.001);
+
+ memory.read(ADDRESS_UP_MARGIN,UpMargin);
+ Up_PID.setMargin(UpMargin);
+
+ memory.read(ADDRESS_UPPER_KP,U_Kc);
+ Up_PID.setKp(U_Kc);
+ memory.read(ADDRESS_UPPER_KI,U_Ki_gain);
+ Up_PID.setKi(U_Ki_gain);
+ memory.read(ADDRESS_UPPER_KD,U_Kd_gain);
+ Up_PID.setKd(U_Kd_gain);
+
+ //Up_PID.setMode(0);
+ //Up_PID.setInputLimits(18,62);
+ //Up_PID.setOutputLimits(0,1);
+}
+//******************************************************/
+/*
+void SET_LowerPID()
+{
+ Lower.period(0.001);
+
+ memory.read(ADDRESS_LOW_MARGIN,LowMargin);
+ Low_PID.setMargin(LowMargin);
+
+ memory.read(ADDRESS_LOWER_KP,L_Kc);
+ Low_PID.setKp(L_Kc);
+ memory.read(ADDRESS_LOWER_KI,L_Ki_gain);
+ Low_PID.setKi(L_Ki_gain);
+
+ memory.read(ADDRESS_LOWER_KD,L_Kd_gain);
+ Low_PID.setKd(L_Kd_gain);
+ //Low_PID.setMode(0);
+ //Low_PID.setInputLimits(0,127); // set range
+ //Low_PID.setOutputLimits(0,1);
+}
+//******************************************************/
+/*
+void Move_Upper()
+{
+ Read_Encoder(EncoderA);
+ Upper_Position = (float)Get_EnValue(data);
+#ifdef DEBUG_UP
+ printf("read_encode = 0x%2x \n\r",data);
+ printf("Setpoint = %d\n\r",Upper_SetPoint);
+ printf("Upper_Position = %f\n\r",Upper_Position);
+#endif
+ Up_PID.setCurrent(Upper_Position);
+ float speed =Up_PID.compute();
+#ifdef DEBUG_UP
+ printf("E_n= %f\n\r",Up_PID.getErrorNow());
+ printf("Kp = %f\n\r",Up_PID.getKp());
+ printf("speed = %f\n\n\n\r",speed);
+#endif
+ Upper.speed(speed);
+}
+//******************************************************/
+/*
+void Move_Lower()
+{
+ Read_Encoder(EncoderB);
+ Lower_Position = (float)Get_EnValue(data);
+ Low_PID.setCurrent(Lower_Position);
+#ifdef DEBUG_LOW
+ printf("read_encode = 0x%2x \n\r",data);
+ printf("Setpoint = %d\n\r",Lower_SetPoint);
+ printf("Upper_Position = %f\n\r",Lower_Position);
+#endif
+
+ float speed =Low_PID.compute();
+#ifdef DEBUG_LOW
+ printf("E_n= %f\n\r",Low_PID.getErrorNow());
+ printf("Kp = %f\n\r",Low_PID.getKp());
+ printf("speed = %f\n\n\n\r",speed);
+#endif
+ Lower.speed(speed);
+}
+//******************************************************/
+
+
+void Rc()
+{
+ myled =1;
+ uint8_t data_array[30];
+ uint8_t id=0;
+ uint8_t ins=0;
+ uint8_t status=0xFF;
+
+
+
+ status = com.ReceiveCommand(&id,data_array,&ins);
+ PC.printf("status = 0x%02x\n\r",status);
+ if(status == ANDANTE_ERRBIT_NONE) {
+ CmdCheck((int16_t)id,data_array,ins);
+ PC.printf("s******************************");
+ }
+
+}
+/*******************************************************/
int main()
{
- if(!button) {
- while(!button);
- DebugMode(); //-->Debug.h
+ PC.baud(115200);
+ printf("******************");
+ /*
+ while(1)
+ {
+ Read_Encoder(EncoderA);
+ Upper_Position = Get_EnValue(data);
+ Read_Encoder(EncoderB);
+ Lower_Position = Get_EnValue(data);
+ PC.printf("Upper Position : %f\n",Upper_Position);
+ PC.printf("Lower_Position : %f\n",Lower_Position);
+ wait(0.5);
}
-
- UI ui(SW_WALK1,SW_WALK2,SW_WALK3,SW_SWEEP,SW_WATER,EMERGENCY);
-
- pc.printf("System Start\n");
+ */
+
+
+ //Recieve.attach(&Rc,0.025);
+ //Start_Up();
+
+ //SET_UpperPID();
+ // SET_LowerPID();
+
+ // printf("BEAR MOTION ID:0x%02x\n\r",MY_ID);
+ /*
while(1)
{
- while(!ui.getEmergencyStatus()) {
- ui.RunSystem();
+
+ Upper.speed(-1);
+ wait_ms(400);
+ Upper.speed(1);
+ wait_ms(400);
+ Upper.break();
+
+ Lower.speed(-1.0);
+ wait_ms(401);
+ Lower.speed(1.0);
+ wait_ms(401);
+ Lower.break();
}
+ */
+
+ // counterUP.start();
+// counterLOW.start();
+
+ while(1) {
+
+ /*
+ //printf("timer = %d\n\r",counter.read_ms());
+ if(counterUP.read_ms() > 1400) {
+
+ if(Upper_SetPoint < 53) {
+ Upper_SetPoint++;
+ } else
+ Upper_SetPoint =18;
+
+ counterUP.reset();
+
+ }
+
+ if(counterLOW.read_ms() > 700) {
+
+ if(Lower_SetPoint < 100) {
+ Lower_SetPoint++;
+ } else
+ Lower_SetPoint =8;
+
+ counterLOW.reset();
+
+ }
+ */
+ // myled =1;
+ //wait_ms(10);
+///////////////////////////////////////////////////// start
+ //Set Set_point
+ // Up_PID.setGoal(Upper_SetPoint);
+ // Low_PID.setGoal(Lower_SetPoint);
+
+ //Control Motor
+ // Move_Upper();
+ // Move_Lower();
+///////////////////////////////////////////////////// stop =306us
+ //uint8_t aaa[1]={10};
+ //com.sendBodyWidth(0x01,aaa);
+ Rc();
+ //wait_ms(1);
}
}
-void DebugMode()
-{
- float temp;
- int option;
- bool first_time = true;
- Debug debug(SERIAL_TX,SERIAL_RX);
- do {
- debug.PrintListMode();
- option = debug.ScanInputData(1);
+
+
- if(option == 1) {
- do {
- temp = debug.Mode1();
- debug.PrintAll(temp);
- } while(temp!=9999);
- }
+
- else if(option == 2) {
- do {
- if(first_time==false) {
- temp = debug.Mode2();
- debug.PrintAll(temp);
- } else {
- temp = debug.Mode2();
- if(temp!=0) first_time = false;
- }
- if(temp==9999) first_time = true;
- } while(temp!=9999);
- }
- debug.setChange();
- } while(option!=9999);
-}
\ No newline at end of file
+void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
+{
+ PC.printf("cmdcheck\n");
+ if(id==MY_ID) {
+ switch (ins) {
+ case PING: {
+ break;
+ }
+ case WRITE_DATA: {
+ switch (command[0]) {
+ case ID: {
+ ///
+ MY_ID = (int16_t)command[1];
+ break;
+ }
+ case SET_VELOCITY_LEFT: {
+ //
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ VL=Int/1000;
+ PC.printf("VL=%f /n",VL);
+ break;
+ }
+ case SET_VELOCITY_RIGHT: {
+ //
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ VR=Int/1000;
+ break;
+ }
+ case SET_VELOCITY_MAX_LEFT: {
+ //
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ VLmax=Int/1000;
+ break;
+ }
+ case SET_VELOCITY_MAX_RIGHT: {
+ //
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ VRmax=Int/1000;
+ PC.printf("VRmax = %f",VRmax);
+ PC.printf("*****************************");
+ break;
+ }
+ case SET_KP_LEFT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KP_LEFT=Int/1000;
+ break;
+ }
+ case SET_KI_LEFT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KI_LEFT=Int/1000;
+ break;
+ }
+ case SET_KD_LEFT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KD_LEFT=Int/1000;
+ break;
+ }
+ case SET_KP_RIGHT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KP_RIGHT=Int/1000;
+ break;
+ }
+ case SET_KI_RIGHT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KI_RIGHT=Int/1000;
+ break;
+ }
+ case SET_KD_RIGHT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KD_RIGHT=Int/1000;
+ break;
+ }
+ }
+ } break;
+ case READ_DATA: {
+ switch (command[0]) {
+ case GET_LIDAR: {
+
+ break;
+ }
+ case GET_BATTERY: {
+
+ break;
+ }
+ case GET_VELOCITY_LEFT: {
+ uint8_t intVelo_L[2],floatVelo_L[2];
+ com.FloatSep(valocity1,intVelo_L,floatVelo_L);
+
+
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = MY_ID;
+ package.length = 6;
+ package.instructionErrorId = WRITE_DATA;
+ package.parameter[0]=intVelo_L[0];
+ package.parameter[1]=intVelo_L[1];
+ package.parameter[2]=floatVelo_L[0];
+ package.parameter[3]=floatVelo_L[1];
+
+ rs485_dirc1=1;
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
+
+ break;
+ }
+ case GET_VELOCITY_RIGHT : {
+ uint8_t intVelo_R[2],floatVelo_R[2];
+ com.FloatSep(valocity2,intVelo_R,floatVelo_R);
+
+
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = MY_ID;
+ package.length = 6;
+ package.instructionErrorId = WRITE_DATA;
+ package.parameter[0]=intVelo_R[0];
+ package.parameter[1]=intVelo_R[1];
+ package.parameter[2]=floatVelo_R[0];
+ package.parameter[3]=floatVelo_R[1];
+
+ rs485_dirc1=1;
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
+ break;
+ }
+ case GET_KP_LEFT: {
+ uint8_t intKPL[2],floatKPL[2];
+ com.FloatSep(KP_LEFT,intKPL,floatKPL);
+
+
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = MY_ID;
+ package.length = 6;
+ package.instructionErrorId = WRITE_DATA;
+ package.parameter[0]=intKPL[0];
+ package.parameter[1]=intKPL[1];
+ package.parameter[2]=floatKPL[0];
+ package.parameter[3]=floatKPL[1];
+
+ rs485_dirc1=1;
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
+ break;
+ }
+ case GET_KI_LEFT: {
+ uint8_t intKIL[2],floatKIL[2];
+ com.FloatSep(KI_LEFT,intKIL,floatKIL);
+
+
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = MY_ID;
+ package.length = 6;
+ package.instructionErrorId = WRITE_DATA;
+ package.parameter[0]=intKIL[0];
+ package.parameter[1]=intKIL[1];
+ package.parameter[2]=floatKIL[0];
+ package.parameter[3]=floatKIL[1];
+
+ rs485_dirc1=1;
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
+ break;
+ }
+ case GET_KD_LEFT: {
+ uint8_t intKDL[2],floatKDL[2];
+ com.FloatSep(KD_LEFT,intKDL,floatKDL);
+
+
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = MY_ID;
+ package.length = 6;
+ package.instructionErrorId = WRITE_DATA;
+ package.parameter[0]=intKDL[0];
+ package.parameter[1]=intKDL[1];
+ package.parameter[2]=floatKDL[0];
+ package.parameter[3]=floatKDL[1];
+
+ rs485_dirc1=1;
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
+ break;
+ }
+ case GET_KP_RIGHT: {
+ uint8_t intKDR[2],floatKDR[2];
+ com.FloatSep(KP_RIGHT,intKDR,floatKDR);
+
+
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = MY_ID;
+ package.length = 6;
+ package.instructionErrorId = WRITE_DATA;
+ package.parameter[0]=intKDR[0];
+ package.parameter[1]=intKDR[1];
+ package.parameter[2]=floatKDR[0];
+ package.parameter[3]=floatKDR[1];
+
+ rs485_dirc1=1;
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
+ break;
+ }
+ case GET_KI_RIGHT: {
+ uint8_t intKIR[2],floatKIR[2];
+ com.FloatSep(KI_RIGHT,intKIR,floatKIR);
+
+
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = MY_ID;
+ package.length = 6;
+ package.instructionErrorId = WRITE_DATA;
+ package.parameter[0]=intKIR[0];
+ package.parameter[1]=intKIR[1];
+ package.parameter[2]=floatKIR[0];
+ package.parameter[3]=floatKIR[1];
+
+ rs485_dirc1=1;
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
+ break;
+ }
+ case GET_KD_RIGHT: {
+ uint8_t intKDR[2],floatKDR[2];
+ com.FloatSep(KD_RIGHT,intKDR,floatKDR);
+
+
+ ANDANTE_PROTOCOL_PACKET package;
+
+ package.robotId = MY_ID;
+ package.length = 6;
+ package.instructionErrorId = WRITE_DATA;
+ package.parameter[0]=intKDR[0];
+ package.parameter[1]=intKDR[1];
+ package.parameter[2]=floatKDR[0];
+ package.parameter[3]=floatKDR[1];
+
+ rs485_dirc1=1;
+ wait_us(RS485_DELAY);
+ com1->sendCommunicatePacket(&package);
+
+ break;
+ }
+ }
+ }break;
+
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/motor/Motor.cpp Mon Mar 21 20:21:12 2016 +0000
@@ -0,0 +1,61 @@
+/* mbed simple H-bridge motor controller
+ * Copyright (c) 2007-2010, sford
+ *
+ * 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.
+ */
+
+#include "Motor.h"
+
+Motor::Motor(PinName pwm, PinName fwd, PinName rev):
+ _pwm(pwm), _fwd(fwd), _rev(rev) {
+
+ // Set initial condition of PWM
+ _pwm.period(0.001);
+ _pwm = 0;
+
+ // Initial condition of output enables
+ _fwd = 0;
+ _rev = 0;
+}
+
+void Motor::speed(float speed) {
+ _fwd = (speed > 0.0);
+ _rev = (speed < 0.0);
+ _pwm = abs(speed);
+}
+
+void Motor::period(float period){
+
+ _pwm.period(period);
+
+}
+
+void Motor::brake(int highLow){
+
+ if(highLow == BRAKE_HIGH){
+ _fwd = 1;
+ _rev = 1;
+ }
+ else if(highLow == BRAKE_LOW){
+ _fwd = 0;
+ _rev = 0;
+ }
+
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/motor/Motor.h Mon Mar 21 20:21:12 2016 +0000
@@ -0,0 +1,76 @@
+/* mbed simple H-bridge motor controller
+ * Copyright (c) 2007-2010, sford
+ *
+ * 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_MOTOR_H
+#define MBED_MOTOR_H
+
+#include "mbed.h"
+
+#define BRAKE_HIGH 1
+#define BRAKE_LOW 0
+
+/** Interface to control a standard DC motor
+ * with an H-bridge using a PwmOut and 2 DigitalOuts
+ */
+class Motor {
+public:
+
+ /** Create a motor control interface
+ *
+ * @param pwm A PwmOut pin, driving the H-bridge enable line to control the speed
+ * @param fwd A DigitalOut, set high when the motor should go forward
+ * @param rev A DigitalOut, set high when the motor should go backwards
+ */
+ Motor(PinName pwm, PinName fwd, PinName rev);
+
+ /** Set the speed of the motor
+ *
+ * @param speed The speed of the motor as a normalised value between -1.0 and 1.0
+ */
+ void speed(float speed);
+
+ /** Set the period of the pwm duty cycle.
+ *
+ * Wrapper for PwmOut::period()
+ *
+ * @param seconds - Pwm duty cycle in seconds.
+ */
+ void period(float period);
+
+ /** Brake the H-bridge to GND or VCC.
+ *
+ * Defaults to breaking to VCC.
+ *
+ * Brake to GND => inA = inB = 0
+ * Brake to VCC => inA = inB = 1
+ */
+ void brake(int highLow = BRAKE_HIGH);
+
+protected:
+ PwmOut _pwm;
+ DigitalOut _fwd;
+ DigitalOut _rev;
+
+};
+
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/move.cpp Mon Mar 21 20:21:12 2016 +0000
@@ -0,0 +1,52 @@
+#include "mbed.h"
+#include "move.h"
+
+
+
+DigitalOut dir1(PC_10);
+DigitalOut dir2(PC_12);
+PwmOut speeds(PA_5);
+DigitalOut dirr1(PB_6);
+DigitalOut dirr2(PC_7);
+PwmOut speeds2(PA_7);
+DigitalOut relays(PA_8);
+ void move:: movespeed_1(float setpoint,float spd)
+{
+ double dc=0;
+ if(setpoint>=0)
+ {
+ dir1=1;
+ dir2=0;
+ }
+ else
+ {
+ dir1=0;
+ dir2=1;
+ }
+ dc=setpoint+spd;
+ speeds.write(dc);
+
+
+}
+void move:: movespeed_2(float setpoint,float spd)
+{
+ double dc=0;
+ if(setpoint>=0)
+ {
+ dirr1=1;
+ dirr2=0;
+ }
+ else
+ {
+ dirr1=0;
+ dirr2=1;
+ }
+ dc=setpoint+spd;
+ speeds.write(dc);
+
+
+}
+void move::pump(int on_off)
+{
+ relays=on_off;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/move.h Mon Mar 21 20:21:12 2016 +0000
@@ -0,0 +1,12 @@
+#ifndef MOVE_H
+#define MOVE_H
+
+
+class move
+{
+ public: void movespeed_1(float setpoint,float spd);
+ void movespeed_2(float setpoint,float spd);
+ void pump(int on_off);
+
+};
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/pidcontrol.cpp Mon Mar 21 20:21:12 2016 +0000
@@ -0,0 +1,168 @@
+#include "pidcontrol.h"
+
+PID::PID()
+{
+ Kp=1.0f;
+ Ki=0.0f;
+ Kd=0.0f;
+ il=65535.0;
+ margin = 0.0f;
+
+}
+
+PID::PID(float p,float i,float d)
+{
+ Kp=p;
+ Ki=i;
+ Kd=d;
+ il=65535.0;
+ margin =0.0f;
+}
+
+void PID::setGoal(float ref)
+{
+ setpoint = ref;
+}
+
+void PID::setCurrent(float sensor)
+{
+ input = sensor;
+}
+
+float PID::compute()
+{
+
+ e_n = setpoint - input;
+
+ if((e_i < il) && (e_i > -il))
+ {
+ e_i += e_n;
+ }
+ else
+ {
+#ifdef PID_DEBUG
+ printf("il overflow\n\r");
+#endif
+ e_i =il;
+ }
+
+
+ output = (Kp*e_n)+(Ki*e_i)+(Kd*(e_n-e_n_1));
+
+ if(output > 0)
+ {
+ if(output < margin)
+ {
+ output = 0.0;
+ }
+ }
+ else
+ {
+ if(output > -margin)
+ {
+ output = 0.0;
+ }
+ }
+
+ return output;
+}
+
+void PID::setMargin(float gap)
+{
+ margin =gap;
+}
+
+float PID::getMargin()
+{
+ return margin;
+}
+
+
+void PID::setIntegalLimit(float limit)
+{
+ il = limit;
+}
+float PID::getIntegalLimit()
+{
+ return il;
+}
+
+
+float PID::getErrorNow()
+{
+ return e_n;
+}
+
+float PID::getErrorLast()
+{
+ return e_n_1;
+}
+
+float PID::getErrorDiff()
+{
+ return e_n - e_n_1;
+}
+
+float PID::getErrorIntegal()
+{
+ return e_i;
+}
+
+void PID::setKp(float p)
+{
+ if(p < 0.0f)
+ {
+#ifdef PID_DEBUG
+ printf("Kp = 0.0\n\r");
+#endif
+ Kp=0.0;
+ }
+ else
+ {
+ Kp=p;
+ }
+}
+
+void PID::setKi(float i)
+{
+ if(i < 0.0f)
+ {
+#ifdef PID_DEBUG
+ printf("Ki = 0.0\n\r");
+#endif
+ Ki=0.0;
+ }
+ else
+ {
+ Ki=i;
+ }
+}
+void PID::setKd(float d)
+{
+ if(d < 0.0f)
+ {
+#ifdef PID_DEBUG
+ printf("Kd = 0.0\n\r");
+#endif
+ Kd=0.0;
+ }
+ else
+ {
+ Kd=d;
+ }
+}
+
+float PID::getKp()
+{
+ return Kp;
+}
+
+float PID::getKi()
+{
+ return Ki;
+}
+
+float PID::getKd()
+{
+ return Kd;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/pidcontrol.h Mon Mar 21 20:21:12 2016 +0000
@@ -0,0 +1,49 @@
+#ifndef _PIDCONTROL_H_
+#define _PIDCONTROL_H_
+
+#include "mbed.h"
+
+class PID{
+ public:
+ PID();
+ PID(float p,float i,float d);
+ void setGoal(float ref);
+ //float getGoal();
+ void setCurrent(float sensor);
+ float compute();
+
+ void setMargin(float gap);
+ float getMargin();
+ void setIntegalLimit(float limit);
+ float getIntegalLimit();
+
+ float getErrorNow();
+ float getErrorLast();
+ float getErrorDiff();
+ float getErrorIntegal();
+
+ void setKp(float);
+ void setKi(float);
+ void setKd(float);
+
+ float getKp();
+ float getKi();
+ float getKd();
+
+ private:
+ float e_n; //error now
+ float e_n_1; //error last time
+ float e_i; //error integal
+ float il; //integal limit
+ float margin; //output margin
+
+ float Kp,Ki,Kd;
+
+ float setpoint;
+ float input;
+ float output;
+};
+
+
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pinconfig.h Mon Mar 21 20:21:12 2016 +0000 @@ -0,0 +1,39 @@ +#ifndef PIN_CONFIG_H +#define PIN_CONFIG_H + +//upper left +#define PWM_LU A1 +#define CS_LU A2 +#define A_LU PD_2 +#define B_LU A0 + +//lower left +#define PWM_LL PB_9 +#define CS_LL D11 +#define A_LL PB_8 +#define B_LL PC_9 + +//limit switch +#define Lim_LU1 PB_7 +#define Lim_LU2 PC_13 +#define Lim_LL1 PC_14 +#define Lim_LL2 PC_15 +#define Lim_RU1 D2 +#define Lim_RU2 D3 +#define Lim_RL1 D8 +#define Lim_RL2 D9 + +//encoder mode1 +#define Emosi PC_12 +#define Emiso PC_11 +#define Esck PC_10 +#define EncoderA PA_13 +#define EncoderB PA_14 + +//serial comm +#define Tx PA_11 +#define Rx PA_12 +#define Dir PB_5 + +#endif +