Yoshito Onishi
/
MotionerController
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: EthernetNetIf MCP2515 XBee mbed
Diff: main.cpp
- Revision:
- 1:c2f68b0adabb
- Parent:
- 0:6af643b2bf72
- Child:
- 2:0939d8f69886
--- a/main.cpp Fri Jan 18 17:01:02 2013 +0000
+++ b/main.cpp Fri Jan 18 17:07:53 2013 +0000
@@ -43,35 +43,35 @@
//--------------------------------------------------------------
// objects
// Serial
-Serial pc(USBTX, USBRX);
+Serial gPc(USBTX, USBRX);
//--------------------------------------------------------------
// XBee WiFi
-XBeeWiFi xbee(p13, p14, p12, P0_22); // TX, RX, CTS, RTS
-//XBeeWiFi xbee(p13, p14, p12, p11); // TX, RX, CTS, RTS
+XBeeWiFi gXBee(p13, p14, p12, P0_22); // TX, RX, CTS, RTS
+//XBeeWiFi gXBee(p13, p14, p12, p11); // TX, RX, CTS, RTS
//--------------------------------------------------------------
//CAN
#define BUS_SPEED (1000)
-SPI spi(p5, p6, p7); // mosi, miso, sclk
-//MCP2515 can(spi, p15);
-MCP2515 can(spi, p8);
+SPI gSpi(p5, p6, p7); // mosi, miso, sclk
+//MCP2515 gCan(gSpi, p15);
+MCP2515 gCan(gSpi, p8);
-bool received[N_JOINTS][2];
-float __quat__[N_JOINTS+1][4];
-floatAndByte tmpQuat[N_JOINTS+1][4];
+bool gReceived[N_JOINTS][2];
+float __gQuat__[N_JOINTS+1][4];
+floatAndByte gTmpQuat[N_JOINTS+1][4];
//mbed CAN
#define MBED_CAN_BUS_SPEED (1000000)
#define N_MBED_CAN (2)
-CAN mbedCan0(p9, p10);
-CAN mbedCan1(p30, p29);
+CAN gMbedCan0(p9, p10);
+CAN gMbedCan1(p30, p29);
-CANMessage mbedCanMessage[N_MBED_CAN];
+CANMessage gMbedCanMessage[N_MBED_CAN];
//--------------------------------------------------------------
-Timer timer;
-long frame = 0;
+Timer gTimer;
+long gFrame = 0;
//--------------------------------------------------------------
// main()
@@ -85,18 +85,18 @@
//--------------------------------------------------------------
bool initXBee(int baud)
{
- pc.printf("Init XBee WiFi with %d baudrate\r\n", baud);
- xbee.begin(baud);
+ gPc.printf("Init XBee WiFi with %d baudrate\r\n", baud);
+ gXBee.begin(baud);
// set XBee baudrate as 921600!!(1Mbps)
- // then we cann't comunicate with XbeeWiFi using pc
+ // then we cann't comunicate with XbeeWiFi using gPc
// so we should set baudrate under 115200 when we want to comunicate with it
- xbee.baud(921600);
+ gXBee.baud(921600);
if (initXBeeWiFi(20)) {
return false;
} else {
- pc.printf("succeed\r\n");
+ gPc.printf("succeed\r\n");
return true;
}
}
@@ -104,64 +104,64 @@
//--------------------------------------------------------------
void setup()
{
- pc.baud(115200);
- pc.printf("[ram::Motioner]\r\n");
+ gPc.baud(115200);
+ gPc.printf("[ram::Motioner]\r\n");
if (!initXBee(115200))
if (!initXBee(921600))
error("XBee init failure\r\n");
IpAddr ipaddr, netmask, gateway, nameserver;
- xbee.getAddress(ipaddr, netmask, gateway, nameserver);
- pc.printf("IP address %d.%d.%d.%d\r\n", ipaddr[0], ipaddr[1], ipaddr[2], ipaddr[3]);
+ gXBee.getAddress(ipaddr, netmask, gateway, nameserver);
+ gPc.printf("IP address %d.%d.%d.%d\r\n", ipaddr[0], ipaddr[1], ipaddr[2], ipaddr[3]);
if (ipaddr == 0) {
- pc.printf("not configure\r\n");
+ gPc.printf("not configure\r\n");
error("");
}
// CAN
- //spi.format( 8, 1 ); //Arduino's SPI Mode = 1
- spi.frequency( 8000000 ); //8MHz
+ //gSpi.format( 8, 1 ); //Arduino's SPI Mode = 1
+ gSpi.frequency( 8000000 ); //8MHz
- pc.printf("CAN reset\r\n");
- can.reset();
- pc.printf("CAN baudConfig\r\n");
- can.baudConfig(BUS_SPEED);
- //can.configRate(CAN_500KBPS_8MHZ);
- pc.printf("CAN setMask\r\n");
- //can.setMask(MASK_SID_CPL_MATCH);//test no mask
- //can.setMask(MASK_SID_ALL_HIT);//test no mask
+ gPc.printf("CAN reset\r\n");
+ gCan.reset();
+ gPc.printf("CAN baudConfig\r\n");
+ gCan.baudConfig(BUS_SPEED);
+ //gCan.configRate(CAN_500KBPS_8MHZ);
+ gPc.printf("CAN setMask\r\n");
+ //gCan.setMask(MASK_SID_CPL_MATCH);//test no mask
+ //gCan.setMask(MASK_SID_ALL_HIT);//test no mask
//enable RX1 and RX0 buffers and rollover
- can.bitModify(RXB0CTRL, RXB_RX_MASK | RXB_BUKT_MASK, RXB_RX_STDEXT | RXB_BUKT_MASK );
- can.bitModify(RXB1CTRL, RXB_RX_MASK, RXB_RX_STDEXT);
+ gCan.bitModify(RXB0CTRL, RXB_RX_MASK | RXB_BUKT_MASK, RXB_RX_STDEXT | RXB_BUKT_MASK );
+ gCan.bitModify(RXB1CTRL, RXB_RX_MASK, RXB_RX_STDEXT);
- //can.setRegister(MCP_RXM0SIDL, 0x11);
- //can.setRegister(MCP_RXM0SIDH, 0x32);
+ //gCan.setRegister(MCP_RXM0SIDL, 0x11);
+ //gCan.setRegister(MCP_RXM0SIDH, 0x32);
byte addrHi;
byte addrLo;
- pc.printf("readRegister\r\n");
- can.readRegister(RXM0SIDH, &addrHi);
- pc.printf("readRegister\r\n");
- can.readRegister(RXM0SIDL, &addrLo);
+ gPc.printf("readRegister\r\n");
+ gCan.readRegister(RXM0SIDH, &addrHi);
+ gPc.printf("readRegister\r\n");
+ gCan.readRegister(RXM0SIDL, &addrLo);
- pc.printf("CAN address");
- pc.printf("%02x",addrHi);
- pc.printf("%02x",addrLo);
- pc.printf("\r\n");
+ gPc.printf("CAN address");
+ gPc.printf("%02x",addrHi);
+ gPc.printf("%02x",addrLo);
+ gPc.printf("\r\n");
- //can.setMode(LOOPBACK);
- can.setMode(NORMAL);
+ //gCan.setMode(LOOPBACK);
+ gCan.setMode(NORMAL);
//
- mbedCan0.frequency(MBED_CAN_BUS_SPEED);
- mbedCan1.frequency(MBED_CAN_BUS_SPEED);
+ gMbedCan0.frequency(MBED_CAN_BUS_SPEED);
+ gMbedCan1.frequency(MBED_CAN_BUS_SPEED);
wait(1.0f);
- timer .start();
+ gTimer .start();
//freezeTimer.start();
//freezeChecker.attach(&freezeCheck, 0.3f);
@@ -196,8 +196,8 @@
unsigned short frame_id = 0x0040;
- can.load_ff_0(length,frame_id,frame_data);
- can.send_0();
+ gCan.load_ff_0(length,frame_id,frame_data);
+ gCan.send_0();
}
//--------------------------------------------------------------
@@ -211,9 +211,9 @@
msg.id = 0x0040;
if (id==0)
- mbedCan0.write(msg);
+ gMbedCan0.write(msg);
else
- mbedCan1.write(msg);
+ gMbedCan1.write(msg);
}
//--------------------------------------------------------------
@@ -221,13 +221,13 @@
//--------------------------------------------------------------
void updateCAN(float time)
{
- const float canBegin = timer.read();
+ const float canBegin = gTimer.read();
int nCanUpdate = 0;
beginTransmitQuaternion();
// loop for update can
- while (timer.read()-canBegin < time) {
+ while (gTimer.read()-canBegin < time) {
//CAN
byte nodeId = 0x00;
byte length = 0x00, rx_status = 0x00;
@@ -237,14 +237,14 @@
for (int i=0; i<8; i++)
frame_data[i] = 0x00;
- rx_status = can.readStatus();
+ rx_status = gCan.readStatus();
if ((rx_status & 0x01) == 0x01) {
- can.readDATA_ff_0(&length,frame_data,&frame_id);
- //pc.printf("0");
+ gCan.readDATA_ff_0(&length,frame_data,&frame_id);
+ //gPc.printf("0");
} else if ((rx_status & 0x02) == 0x02) {
- can.readDATA_ff_1(&length,frame_data,&frame_id);
- //pc.printf("1");
+ gCan.readDATA_ff_1(&length,frame_data,&frame_id);
+ //gPc.printf("1");
}
if (((rx_status & 0x01) == 0x01) || ((rx_status & 0x02) == 0x02)) {
@@ -253,19 +253,19 @@
nodeId = frame_id >> 3;
uint8_t slot = frame_id & 0x01;
- //if (frame % 120 == 0)
- // pc.printf("%d %d, ", frame_id, nodeId);
+ //if (gFrame % 120 == 0)
+ // gPc.printf("%d %d, ", frame_id, nodeId);
- received[nodeId][slot] = true;
+ gReceived[nodeId][slot] = true;
// 0to18 Joints and RX buffer*2(quaternion xy or zw) and 8 byte CAN data
if (nodeId < N_JOINTS && slot < 2 && length == 8) {
- //pc.printf("%x, %x\n\r", nodeId, slot);
+ //gPc.printf("%x, %x\n\r", nodeId, slot);
for (int q=0; q<2; q++) { //quaternion x, y or z, w
for (int f=0; f<4; f++) { //float byte 0 - 4
//[node id][quaternion x, y or z, w][float bytes 0 - 4]
- tmpQuat[nodeId][q+slot*2].b[f] = frame_data[f+q*4];
+ gTmpQuat[nodeId][q+slot*2].b[f] = frame_data[f+q*4];
}
}
}
@@ -278,26 +278,26 @@
byte nodeId = 0x00;
if (canId == 0)
- can = &mbedCan0;
+ can = &gMbedCan0;
else
- can = &mbedCan1;
+ can = &gMbedCan1;
- if (can->read(mbedCanMessage[canId])) {
+ if (can->read(gMbedCanMessage[canId])) {
- CANMessage &msg = mbedCanMessage[canId];
+ CANMessage &msg = gMbedCanMessage[canId];
nodeId = msg.id >> 3;
uint8_t slot = msg.id & 0x01;
- received[nodeId][slot] = true;
+ gReceived[nodeId][slot] = true;
// 0to18 Joints and RX buffer*2(quaternion xy or zw) and 8 byte CAN data
if (nodeId < N_JOINTS && slot < 2 && msg.len == 8) {
- //pc.printf("%x, %x\n\r", nodeId, slot);
+ //gPc.printf("%x, %x\n\r", nodeId, slot);
for (int q=0; q<2; q++) { //quaternion x, y or z, w
for (int f=0; f<4; f++) { //float byte 0 - 4
//[node id][quaternion x, y or z, w][float bytes 0 - 4]
- tmpQuat[nodeId][q+slot*2].b[f] = msg.data[f+q*4];
+ gTmpQuat[nodeId][q+slot*2].b[f] = msg.data[f+q*4];
}
}
}
@@ -307,18 +307,18 @@
nCanUpdate++; //how many times could we update can?
}// loop for update can
- //if (frame % 120 == 0)
- // pc.printf("\n\r");
+ //if (gFrame % 120 == 0)
+ // gPc.printf("\n\r");
- if (frame % 240 == 0) {
- //pc.printf("\r\n");
- const float elapsed = timer.read()-canBegin;
- pc.printf("c%f/%d\r\n", elapsed, nCanUpdate);
+ if (gFrame % 240 == 0) {
+ //gPc.printf("\r\n");
+ const float elapsed = gTimer.read()-canBegin;
+ gPc.printf("c%f/%d\r\n", elapsed, nCanUpdate);
for (int i=0; i<N_JOINTS; i++)
- if (received[i][0] || received[i][1])
- pc.printf("%d:%d%d, ", i, received[i][0], received[i][1]);
- //pc.printf("%x: %f, %f, %f, %f\n\r", i, ___quat___[i][0].f, ____quat____[i][1].f, ____quat____[i][2].f, ___quat___[i][3].f);
- pc.printf("\r\n");
+ if (gReceived[i][0] || gReceived[i][1])
+ gPc.printf("%d:%d%d, ", i, gReceived[i][0], gReceived[i][1]);
+ //gPc.printf("%x: %f, %f, %f, %f\n\r", i, ___gQuat___[i][0].f, ____gQuat____[i][1].f, ____gQuat____[i][2].f, ___gQuat___[i][3].f);
+ gPc.printf("\r\n");
}
endTransmitQuaternion();
@@ -327,8 +327,8 @@
//--------------------------------------------------------------
void updateSerial()
{
- while(pc.readable()) {
- char c = pc.getc();
+ while(gPc.readable()) {
+ char c = gPc.getc();
switch (c) {
};
@@ -352,14 +352,14 @@
//buf[i] = (char)('0'+i%70);
for (int i=0; i<N_JOINTS; i++) {
- if (received[i][0] && received[i][1]) {
+ if (gReceived[i][0] && gReceived[i][1]) {
for (int j=0; j<4; j++) {
- __quat__[i][j] = tmpQuat[i][j].f;
+ __gQuat__[i][j] = gTmpQuat[i][j].f;
}
}
}
- uint8_t *buf = (uint8_t *)__quat__;
+ uint8_t *buf = (uint8_t *)__gQuat__;
buf[DATA_LEN-4] = (char)'a';
buf[DATA_LEN-3] = (char)'b';
@@ -376,20 +376,20 @@
//data.setPayloadLength(strlen(buf));
data.setPayload(buf);
data.setPayloadLength(DATA_LEN);
- data.setFrameId(xbee.getNextFrameId());//0
- xbee.send(data);
+ data.setFrameId(gXBee.getNextFrameId());//0
+ gXBee.send(data);
- //pc.printf("len:%d\r\n",strlen(buf));
- const int r = xbee.getWiResponse(TX_STATUS_RESPONSE, data.getFrameId());
+ //gPc.printf("len:%d\r\n",strlen(buf));
+ const int r = gXBee.getWiResponse(TX_STATUS_RESPONSE, data.getFrameId());
/// error
if (r==-1) {
- pc.printf("Fatal error! XBee WiFi didn't responce!\r\nTrying to reset modem again.");
+ gPc.printf("Fatal error! XBee WiFi didn't responce!\r\nTrying to reset modem again.");
if (initXBeeWiFi(20)) {
- pc.printf("Failure...");
+ gPc.printf("Failure...");
error("");
} else {
- pc.printf("succeed\r\n");
+ gPc.printf("succeed\r\n");
}
}
}
@@ -397,24 +397,24 @@
//--------------------------------------------------------------
void loop()
{
- const float transBegin = timer.read();
+ const float transBegin = gTimer.read();
updateSerial();
updateXBeeWiFi();
- const float elapsed = timer.read() - transBegin;
+ const float elapsed = gTimer.read() - transBegin;
const float frameTime = 1.0f/60.0f;
const float t = frameTime-elapsed;
- if (frame % 120 == 0)
- pc.printf("x%f\r\n", elapsed);
+ if (gFrame % 120 == 0)
+ gPc.printf("x%f\r\n", elapsed);
- frame++;
+ gFrame++;
for (int i=0; i<N_JOINTS; i++)
for (int j=0; j<2; j++)
- received[i][j] = false;
+ gReceived[i][j] = false;
updateCAN(t);
}
@@ -425,55 +425,55 @@
{
int i, r;
- pc.printf("Reset XBee Wi-Fi\r\n");
- r = xbee.reset();
+ gPc.printf("Reset XBee Wi-Fi\r\n");
+ r = gXBee.reset();
if (r < 0) {
- pc.printf("Error reset %d\r\n", r);
+ gPc.printf("Error reset %d\r\n", r);
return -1;
}
- pc.printf("Get Responce from XBee Wi-Fi\r\n");
- xbee.getWiResponse(MODEM_STATUS_RESPONSE, 5000);
+ gPc.printf("Get Responce from XBee Wi-Fi\r\n");
+ gXBee.getWiResponse(MODEM_STATUS_RESPONSE, 5000);
- pc.printf("Setup XBee Wi-Fi %d\r\n", r);
- r = xbee.setup(SECURITY, SSID, PASSPHRASE);
- //r = xbee.setup(SSID);
+ gPc.printf("Setup XBee Wi-Fi %d\r\n", r);
+ r = gXBee.setup(SECURITY, SSID, PASSPHRASE);
+ //r = gXBee.setup(SSID);
if (r < 0) {
- pc.printf("Error setup %d\r\n", r);
+ gPc.printf("Error setup %d\r\n", r);
return -1;
}
for (i = 0; i < timeout; i ++) {
wait(1.0f);
- r = xbee.getStatus();
- pc.printf("Status %02x: ", r);
+ r = gXBee.getStatus();
+ gPc.printf("Status %02x: ", r);
switch (r) {
case JOINED_AP:
- pc.printf("Successfully joined an access point.\r\n");
+ gPc.printf("Successfully joined an access point.\r\n");
return 0;
case INITIALIZATION:
- pc.printf("WiFi initialization in progress.\r\n");
+ gPc.printf("WiFi initialization in progress.\r\n");
break;
case SSID_NOT_FOUND:
- pc.printf("SSID not found.\r\n");
+ gPc.printf("SSID not found.\r\n");
return -1;
case SSID_NOT_CONFIGURED:
- pc.printf("SSID not configured.\r\n");
+ gPc.printf("SSID not configured.\r\n");
return -1;
case JOIN_FAILED:
- pc.printf("SSID join failed.\r\n");
+ gPc.printf("SSID join failed.\r\n");
return -1;
case WAITING_IPADDRESS:
- pc.printf("Waiting for IP configuration.\r\n");
+ gPc.printf("Waiting for IP configuration.\r\n");
break;
case WAITING_SOCKETS:
- pc.printf("Listening sockets are being set up.\r\n");
+ gPc.printf("Listening sockets are being set up.\r\n");
break;
case SCANNING_SSID:
- pc.printf("Currently scanning for SSID.\r\n");
+ gPc.printf("Currently scanning for SSID.\r\n");
break;
default:
- pc.printf("\r\n");
+ gPc.printf("\r\n");
break;
}
}
