robo 8080
RCBControllerでモータを制御します。うおーるぼっとも動かせました。
Dependencies: BLE_API TB6612FNG2 mbed nRF51822
Fork of
BLE_RCBController2
by 
Junichi Katsu
うまく接続できない時は、iPhone/iPadのBluetoothをOFF－＞ONしてキャッシュをクリアしてみてください。
ライブラリ類をUpdateするとコンパイル出来なくなります。インポートした物をそのまま使って下さい。
RCBControllerでうおーるぼっとを操縦する例
This robot has switch, line sensors and motors. It controls by mbed.
RCBControllerでの操縦は次の4種類あります。 それぞれうおーるぼっとの動きが異なりますので試してみてください。
- 左十字ボタン
- 左のみアナログ
- 右のみアナログ
- 両方アナログ
うおーるぼっと(LPC1768のソケット)とHRM1017の接続はこれです。
LPC1768 ー HRM1017
p11 ーーー P0_0
p12 ーーー P0_1
p13 ーーー P0_28
p14 ーーー P0_29
p21 ーーー P0_30
p22 ーーー P0_25
GND ーーー GND
HRM1017の電源はうおーるぼっとのUSBコネクタからとります。
main.cpp
- Committer:
- robo8080
- Date:
- 2014-09-17
- Revision:
- 5:1c04bd9f8457
- Parent:
- 2:dd85fdc18224
File content as of revision 5:1c04bd9f8457:
#include "mbed.h"
#include "BLEDevice.h"
#include "RCBController.h"
#include "TB6612.h"
#define DBG 0
const static char DEVICE_NAME[] = "mbed HRM1017";
BLEDevice ble;
Serial pc(USBTX, USBRX);
/* LEDs for indication: */
DigitalOut ConnectStateLed(LED1);
PwmOut ControllerStateLed(LED2);
TB6612 left(P0_30,P0_1,P0_0);
TB6612 right(P0_25,P0_29,P0_28);
#define PI 3.141592
#define neutralAngle 25
#define neutralRange 5
#define angleRange 30.0
#define handlingIntensity 0.7
float AccSin2Deg(uint8_t acc) // acc : -90 ... 90
{
return (float)asin((float)(acc-128.0f)/127.0f)*180.0f/PI;
}
float Acc2Speed(uint8_t acc) // acc : 0 ... 255
{
int deg = neutralAngle+(int)AccSin2Deg(acc);
if(deg>-neutralRange && deg<neutralRange)deg=0;
float speed=100*(float)deg/angleRange;
if(speed>100){
speed=100;
} else if(speed<-100){
speed=-100;
}
return speed;
}
void RCBCon(uint8_t *buffer, uint16_t buffer_size)
{
uint16_t game_pad;
game_pad = (buffer[0] << 8) | buffer[1];
// pc.printf("game_pad : %04X\n",game_pad);
float AccX = (100.0f/90.0f)*AccSin2Deg(buffer[6]);//((float)buffer[6] / 255.0)*200.0 - 100.0;
// float AccY = AccSin2Deg(buffer[7]);//((float)buffer[7] / 255.0)*200.0 - 100.0;
float AccY = Acc2Speed(buffer[7]);
// float AccZ = (100.0f/90.0f)*AccSin2Deg(buffer[8]);((float)buffer[8] / 255.0)*200.0 - 100.0;
// pc.printf("acc X : %f Y : %f Z : %f\n",acc[0],acc[1],acc[2]);
float LeftStickX = ((float)buffer[2] / 255.0)*200.0 - 100.0;
float RightStickX = ((float)buffer[4] / 255.0)*200.0 - 100.0;
float LeftStickY = ((float)buffer[3] / 255.0)*200.0 - 100.0;
float RightStickY = ((float)buffer[5] / 255.0)*200.0 - 100.0;
uint8_t status = buffer[9];
if((status & 0x60) == 0x20) { // Accelerometer ON
float leftData;
float rightData;
float xHandling=(float)AccX*handlingIntensity;
if(game_pad != 0x0020) { // A button
right = 0;
left = 0;
} else {
leftData = AccY;
rightData = AccY;
if(AccY==0) {
leftData= AccX;
rightData= -AccX;
} else if(AccY>0) {
if(AccX>0) {
rightData=AccY-(int)xHandling; //r-x
} else {
leftData=AccY+(int)xHandling; //l-x
}
} else {
if(AccX>0) {
leftData=AccY-(int)xHandling; //l-x
} else {
rightData=AccY+(int)xHandling; //r-x
}
}
left = (int)leftData;
right = (int)rightData;
}
} else if((status & 0x10)&&(status & 0x08)) { // L : Analog R : Analog
left = (int)LeftStickY;
right = (int)RightStickY;
} else if(status & 0x10) { // L : Analog
if((LeftStickX < 15) && (LeftStickX > -15)) {
right = LeftStickY;
left = LeftStickY;
}else if((LeftStickY < 15) && (LeftStickY > -15)) {
right = -LeftStickX;
left = LeftStickX;
} else {
right = 0;
left = 0;
}
} else if(status & 0x08) { // R : Analog
float leftData;
float rightData;
if(RightStickY < 0) {
RightStickX *= -1.0f;
}
if(RightStickX >= 0) {
leftData = RightStickY + RightStickX;
rightData = RightStickY;
} else {
leftData = RightStickY;
rightData = RightStickY - RightStickX;
}
if(leftData > 100) {
leftData = 100;
} else if(leftData < -100) {
leftData = -100;
}
if(rightData > 100) {
rightData = 100;
} else if(rightData < -100) {
rightData = -100;
}
left = (int)leftData;
right = (int)rightData;
} else if((status & 0x60) == 0x40) { // L : Accelerometer
if(game_pad != 0x0020) { // A button
right = 0;
left = 0;
} else {
if((LeftStickX < 15) && (LeftStickX > -15)) {
right = LeftStickY;
left = LeftStickY;
} else if((LeftStickY < 15) && (LeftStickY > -15)) {
right = -LeftStickX;
left = LeftStickX;
} else {
right = 0;
left = 0;
}
}
} else { // Digital button
switch(game_pad)
{
case 0x0001:
left = 50;
right = 50;
break;
case 0x0002:
left = -50;
right = -50;
break;
case 0x0004:
left = 50;
right = -50;
break;
case 0x0008:
left = -50;
right = 50;
break;
default:
left = 0;
right = 0;
}
}
}
/* RCBController Service */
static const uint16_t RCBController_service_uuid = 0xFFF0;
static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
uint8_t RCBControllerPayload[10] = {0,};
GattCharacteristic ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic *ControllerChars[] = {&ControllerChar};
GattService RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
RCBController controller;
void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
{
ConnectStateLed = 0;
#if DBG
pc.printf("Connected\n\r");
#endif
}
void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
ble.startAdvertising();
ConnectStateLed = 1;
left = 0;
right = 0;
#if DBG
pc.printf("Disconnected\n\r");
#endif
}
// GattEvent
void onDataWritten(uint16_t charHandle, const GattCharacteristicWriteCBParams *params)
{
if (charHandle == ControllerChar.getValueAttribute().getHandle()) {
uint16_t bytesRead;
ble.readCharacteristicValue(ControllerChar.getValueAttribute().getHandle(),RCBControllerPayload, &bytesRead);
memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
#if DBG
pc.printf("DATA:%02X %02X %d %d %d %d %d %d %d %02X\n\r",controller.data[0],controller.data[1],controller.data[2],controller.data[3],controller.data[4],
controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
#endif
ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;
RCBCon(&controller.data[0], sizeof(controller));
}
}
/**************************************************************************/
/*!
@brief Program entry point
*/
/**************************************************************************/
int main(void)
{
#if DBG
pc.printf("Start\n\r");
#endif
ConnectStateLed = 1;
left = 0;
right = 0;
ble.init();
ble.setDeviceName((uint8_t *)DEVICE_NAME);
ble.onConnection(onConnected);
ble.onDisconnection(onDisconnected);
ble.onDataWritten(onDataWritten);
/* setup advertising */
ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
(const uint8_t *)"mbed HRM1017", sizeof("mbed HRM1017") - 1);
ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
(const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
ble.startAdvertising();
ble.addService(RCBControllerService);
while (true) {
ble.waitForEvent();
}
}