albatross
/
ControlYokutan2017
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: Control_Yokutan_CANver1 ADXL345_I2C mbed MPU6050
Fork of
Souda_Yokutan_ver528TF
by 
albatross
Revision 26:f14579683f98, committed 2017-02-16
- Comitter:
- YusukeWakuta
- Date:
- Thu Feb 16 16:00:11 2017 +0000
- Branch:
- XBus???
- Parent:
- 25:e8bfb629e1b1
- Child:
- 27:591cf9139ae7
- Commit message:
- ??????
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Feb 15 13:29:20 2017 +0000
+++ b/main.cpp Thu Feb 16 16:00:11 2017 +0000
@@ -24,7 +24,6 @@
#define ERURON_TRIM_INI_L 113 //元々95
#define DRUG_TRIM_INI_L 110
-#define kXBusTx p9
#define kMaxServoNum 1 // 1 - 50
#define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec))
#define kMotionInterval 10 // flame / sec
@@ -46,7 +45,7 @@
DigitalIn LRstatePin(p14);
DigitalIn setTrimPin(p15);
DigitalIn EDstatePin(p16);
-DigitalIn checkMaxDegPin(p17);
+DigitalIn setMaxDegPin(p17);
DigitalOut debugLED(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
@@ -69,10 +68,7 @@
bool INA_FLAG;
bool MPU_FLAG;
-//int16_t acc[3] = {0,0,0};
char gyro_c[6] = {0,0,0,0,0,0};
-//char acc_mean[3][ADXL_MEAN_NUM];
-//int adxl_mean_counter = 0;
void toString();
void receiveDatas();
@@ -80,48 +76,30 @@
static const uint8_t servoChannel = 0x01;
-XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum);
+XBusServo gXBus(p9, NC, NC, kMaxServoNum);
Ticker gTimer;
-bool servoInit(){
+bool servoInit()
+{
drugServo.period_ms(INIT_SERVO_PERIOD_MS);
eruronServo.period_ms(INIT_SERVO_PERIOD_MS);
return true;
}
-//=============================================================
-// XbusIntervalHandler()
-// play motion !
-//=============================================================
-void XbusIntervalHandler()
+void sendDatas()
{
- uint16_t value;
- uint16_t diff = kMotionEndMark - kMotionMinMark;
- value = (uint16_t)(diff * analog.read()) + kMotionMinMark;
- gXBus.setServo(servoChannel, value);
- gXBus.sendChannelDataPacket();
-}
-
-//bool adxlInit(){
-// accelerometer.setPowerControl(0x00);
-// accelerometer.setDataFormatControl(0x0B);
-// accelerometer.setDataRate(ADXL345_3200HZ);
-// accelerometer.setPowerControl(0x08);
-// return true;
-//}
-
-void sendDatas(){
- if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
+ if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) {
}
}
-bool inaInit(){
- if(!VCmonitor.isExist()){
+bool inaInit()
+{
+ if(!VCmonitor.isExist()) {
pc.printf("VCmonitor NOT FOUND\n");
return false;
}
ina_val = 0;
- if(VCmonitor.rawRead(0x00,&ina_val) != 0){
+ if(VCmonitor.rawRead(0x00,&ina_val) != 0) {
pc.printf("VCmonitor READ ERROR\n");
return false;
}
@@ -129,17 +107,30 @@
return true;
}
-XBusError init(){
-
+XBusError initXBus()
+{
XBusError result;
-
- if(!LRstatePin){
+ result = gXBus.start();
+ if (result != kXBusError_NoError) {
+ gXBus.stop();
+ return result;
+ }
+ result = gXBus.addServo(servoChannel, kXbusServoNeutral);
+ if (result != kXBusError_NoError) {
+ gXBus.stop();
+ return result;
+ }
+ return kXBusError_NoError;
+}
+
+void init()
+{
+ if(!LRstatePin) {
eruronTrim = ERURON_TRIM_INI_L;
drugTrim = DRUG_TRIM_INI_L;
eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
drugMoveDeg = DRUG_MOVE_DEG_INI_L;
- }
- else{
+ } else {
eruronTrim = ERURON_TRIM_INI_R;
drugTrim = DRUG_TRIM_INI_R;
eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
@@ -152,136 +143,114 @@
sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
// toStringTicker.attach(&toString,0.5);
receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
-
- //XBus
- // initialize XBus
- result = gXBus.start();
- if (result != kXBusError_NoError) {
- gXBus.stop();
- return result;
- }
- // initialize XBus Servos
- result = gXBus.addServo(servoChannel, kXbusServoNeutral);
- if (result != kXBusError_NoError) {
- gXBus.stop();
- return result;
- }
- return kXBusError_NoError;
+ initXBus();
}
-void updateDatas(){
+void updateDatas()
+{
// if(ADXL_FLAG){
//// accelerometer.getOutput(acc);
// }
- if(INA_FLAG){
+ if(INA_FLAG) {
int tmp = VCmonitor.getVoltage(&V);
tmp = VCmonitor.getCurrent(&C);
}
- if(MPU_FLAG){
+ if(MPU_FLAG) {
mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6);
}
- for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++){
-// toSendDatas[i] = acc[i];
+ for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) {
toSendDatas[i] = gyro_c[i];
}
// toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77;
}
-void receiveDatas(){
- if(can.read(recmsg)){
- for(int i = 0; i < CONTROL_VALUES_NUM; i++){
+void receiveDatas()
+{
+ if(can.read(recmsg)) {
+ for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
controlValues[i] = recmsg.data[i];
if(i<CONTROL_VALUES_NUM-1) floatvalues[i] = controlValues[i];
}
eruronfloat = *(const float *)floatvalues;
led1 = !led1;
- //WriteServo();
}
}
-void toString(){
- for(int i = 0; i <CONTROL_VALUES_NUM; i++){
- pc.printf("%d, ",controlValues[i]);
- }
- pc.printf("\n\r");
-}
-
-double calcPulse(int deg){
+double calcPulse(int deg)
+{
return (0.0006+(deg/180.0)*(0.00235-0.00045));
-
}
-void WriteServo(){
- //if(debugServoPin){
-// led3 = 1;
-// float a = eruronAna.read()*180;
-// float b = drugAna.read()*180;
-// eruronServo.pulsewidth(calcPulse(eruronAna.read()*180));
-// drugServo.pulsewidth(calcPulse(drugAna.read()*180));
-// pc.printf("eruron:%f drug:%f\n\r",a,b);
-// }
-// else{
- // led3 = 0;
-// eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (controlValues[0]-1) ));
- drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
- eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronfloat-1) ));
- //}
+void XbusIntervalHandler()
+{
+ uint16_t value;
+ uint16_t diff = kMotionEndMark - kMotionMinMark;
+ value = (uint16_t)(diff * (eruronTrim + eruronMoveDeg * (eruronfloat-1)) + kMotionMinMark);
+ gXBus.setServo(servoChannel, value);
+ gXBus.sendChannelDataPacket();
+}
+
+ void WriteServo()
+{
+ drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
+ eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronfloat-1) ));
}
-void setTrim(){
+void setTrim()
+{
debugLED = 1;
- if(EDstatePin){
- eruronTrim = eruronAna.read()*180;
- eruronServo.pulsewidth(calcPulse(eruronTrim));
- }
- else{
- drugTrim = drugAna.read()*180;
- drugServo.pulsewidth(calcPulse(drugTrim));
+ if(EDstatePin) {
+ eruronTrim = eruronAna.read();
+ eruronServo.pulsewidth(calcPulse(eruronTrim));
+ } else {
+ drugTrim = drugAna.read()*180;
+ drugServo.pulsewidth(calcPulse(drugTrim));
}
//pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim);
- pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
- pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
+ pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
+ pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
}
-void checkMaxDeg(){
+void setMaxDeg()
+{
led4 = 1;
- float eruronTemp = eruronAna.read()*180;
+ float eruronTemp = eruronAna.read();
float drugTemp = drugAna.read()*180;
- if(EDstatePin){
- eruronServo.pulsewidth(calcPulse(eruronTemp));
- eruronMoveDeg = eruronTemp-eruronTrim;
+ if(EDstatePin) {
+ eruronServo.pulsewidth(calcPulse(eruronTemp));
+ eruronMoveDeg = eruronTemp-eruronTrim;
+ } else {
+ drugServo.pulsewidth(calcPulse(drugTemp));
+ drugMoveDeg = drugTemp-drugTrim;
}
- else{
- drugServo.pulsewidth(calcPulse(drugTemp));
- drugMoveDeg = drugTemp-drugTrim;
- }
- // pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
+ // pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
wait_us(10);
}
-int main(){
+int main()
+{
init();
XBusError result;
-
+
// start motion
gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
-
- while(1){
- while(setTrimPin){
+
+ while(1) {
+ while(setTrimPin) {
setTrim();
}
- while(checkMaxDegPin){
- checkMaxDeg();
+ while (setMaxDegPin) {
+ setMaxDeg();
}
- // pc.printf("eT:%f\n\r",eruronTrim);
- pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
- pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
+ // pc.printf("eT:%f\n\r",eruronTrim);
+ pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
+ pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
led4 = 0;
-
+
debugLED = 0;
//receiveDatas();
// sendDatas();