albatross / Mbed 2 deprecated ControlYokutan2017_2

ControlMainでの変更に対応して、新しくレポジトリを作りました

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017 by albatross

main.cpp@23:d551db88df65, 2017-02-15 (annotated)

Committer:
tsumagari
Date:
Wed Feb 15 13:09:40 2017 +0000
Branch:
XBUS???
Revision:
23:d551db88df65
Parent:
22:b38bc18ec3a1
Child:
25:e8bfb629e1b1
????????????????float??can??????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taurin 0:e052602db102 1 //翼端can program
taurin 0:e052602db102 2 #include "mbed.h"
tsumagari 22:b38bc18ec3a1 3 //#include "ADXL345_I2C.h"
tsumagari 22:b38bc18ec3a1 4 #include "MPU6050.h"
taurin 0:e052602db102 5 #include "INA226.hpp"
taurin 4:450cafd95ac3 6
tsumagari 22:b38bc18ec3a1 7 #define TO_SEND_DATAS_NUM 7
taurin 2:7fcb4f970a02 8 #define INIT_SERVO_PERIOD_MS 20
taurin 4:450cafd95ac3 9 #define WAIT_LOOP_TIME 0.02
tsumagari 23:d551db88df65 10 #define CONTROL_VALUES_NUM sizeof(float) + 1
taurin 0:e052602db102 11 #define TO_SEND_CAN_ID 100
taurin 4:450cafd95ac3 12 #define ADXL_MEAN_NUM 10
taurin 4:450cafd95ac3 13 #define SEND_DATAS_LOOP_TIME 0.1
taurin 16:82310bf7c326 14 #define RECEIVE_DATAS_LOOP_TIME 0.05
taurin 12:fd9d241843f4 15
tsumagari 23:d551db88df65 16 #define ERURON_MOVE_DEG_INI_R 30 // もともと10
YusukeWakuta 18:253f0526b8b3 17 #define DRUG_MOVE_DEG_INI_R 76
YusukeWakuta 20:a5a85df4de13 18 #define ERURON_TRIM_INI_R 97 //元々94　
YusukeWakuta 18:253f0526b8b3 19 #define DRUG_TRIM_INI_R 33
taurin 12:fd9d241843f4 20
tsumagari 23:d551db88df65 21 #define ERURON_MOVE_DEG_INI_L -30 //もともと-7
YusukeWakuta 17:d91ce54dd09a 22 #define DRUG_MOVE_DEG_INI_L -80
YusukeWakuta 20:a5a85df4de13 23 #define ERURON_TRIM_INI_L 113 //元々95
YusukeWakuta 17:d91ce54dd09a 24 #define DRUG_TRIM_INI_L 110
taurin 2:7fcb4f970a02 25
taurin 0:e052602db102 26 CAN can(p30,p29);
taurin 0:e052602db102 27 CANMessage recmsg;
taurin 0:e052602db102 28 Serial pc(USBTX,USBRX);
tsumagari 22:b38bc18ec3a1 29 //ADXL345_I2C accelerometer(p9, p10);
tsumagari 22:b38bc18ec3a1 30 MPU6050 mpu(p9,p10);
taurin 0:e052602db102 31 I2C ina226_i2c(p28,p27);
taurin 0:e052602db102 32 INA226 VCmonitor(ina226_i2c);
taurin 13:5e3b4120dbbf 33 PwmOut drugServo(p22);
taurin 13:5e3b4120dbbf 34 PwmOut eruronServo(p23);
taurin 1:9cc932a16d17 35 DigitalOut led1(LED1);
taurin 4:450cafd95ac3 36 AnalogIn drugAna(p20);
taurin 4:450cafd95ac3 37 AnalogIn eruronAna(p19);
taurin 12:fd9d241843f4 38 DigitalIn LRstatePin(p14);
taurin 4:450cafd95ac3 39 DigitalIn setTrimPin(p15);
taurin 12:fd9d241843f4 40 DigitalIn EDstatePin(p16);
taurin 4:450cafd95ac3 41 DigitalIn checkMaxDegPin(p17);
taurin 4:450cafd95ac3 42 DigitalOut debugLED(LED2);
taurin 4:450cafd95ac3 43 DigitalOut led3(LED3);
taurin 4:450cafd95ac3 44 DigitalOut led4(LED4);
taurin 4:450cafd95ac3 45 Ticker sendDatasTicker;
tsumagari 22:b38bc18ec3a1 46 //Ticker toStringTicker;
taurin 16:82310bf7c326 47 Ticker receiveDatasTicker;
taurin 16:82310bf7c326 48
taurin 0:e052602db102 49 char toSendDatas[TO_SEND_DATAS_NUM];
tsumagari 23:d551db88df65 50 char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4:drug
tsumagari 23:d551db88df65 51 char floatvalues[sizeof(float)];
taurin 12:fd9d241843f4 52 float eruronTrim;
taurin 12:fd9d241843f4 53 float drugTrim;
taurin 12:fd9d241843f4 54 float eruronMoveDeg;
taurin 12:fd9d241843f4 55 float drugMoveDeg;
tsumagari 23:d551db88df65 56 float eruronfloat;
taurin 0:e052602db102 57 unsigned short ina_val;
taurin 0:e052602db102 58 double V,C;
taurin 0:e052602db102 59 bool SERVO_FLAG;
taurin 0:e052602db102 60 bool ADXL_FLAG;
taurin 0:e052602db102 61 bool INA_FLAG;
tsumagari 22:b38bc18ec3a1 62 bool MPU_FLAG;
taurin 0:e052602db102 63
tsumagari 22:b38bc18ec3a1 64 //int16_t acc[3] = {0,0,0};
tsumagari 22:b38bc18ec3a1 65 char gyro_c[6] = {0,0,0,0,0,0};
tsumagari 22:b38bc18ec3a1 66 //char acc_mean[3][ADXL_MEAN_NUM];
tsumagari 22:b38bc18ec3a1 67 //int adxl_mean_counter = 0;
taurin 4:450cafd95ac3 68
taurin 4:450cafd95ac3 69 void toString();
taurin 16:82310bf7c326 70 void receiveDatas();
taurin 16:82310bf7c326 71 void WriteServo();
taurin 0:e052602db102 72
taurin 0:e052602db102 73 bool servoInit(){
taurin 4:450cafd95ac3 74 drugServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 4:450cafd95ac3 75 eruronServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 76 return true;
taurin 0:e052602db102 77 }
taurin 0:e052602db102 78
tsumagari 22:b38bc18ec3a1 79 //bool adxlInit(){
tsumagari 22:b38bc18ec3a1 80 // accelerometer.setPowerControl(0x00);
tsumagari 22:b38bc18ec3a1 81 // accelerometer.setDataFormatControl(0x0B);
tsumagari 22:b38bc18ec3a1 82 // accelerometer.setDataRate(ADXL345_3200HZ);
tsumagari 22:b38bc18ec3a1 83 // accelerometer.setPowerControl(0x08);
tsumagari 22:b38bc18ec3a1 84 // return true;
tsumagari 22:b38bc18ec3a1 85 //}
taurin 0:e052602db102 86
taurin 4:450cafd95ac3 87 void sendDatas(){
taurin 4:450cafd95ac3 88 if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
taurin 4:450cafd95ac3 89 }
taurin 4:450cafd95ac3 90 }
taurin 4:450cafd95ac3 91
taurin 0:e052602db102 92 bool inaInit(){
taurin 0:e052602db102 93 if(!VCmonitor.isExist()){
taurin 0:e052602db102 94 pc.printf("VCmonitor NOT FOUND\n");
taurin 0:e052602db102 95 return false;
taurin 0:e052602db102 96 }
taurin 0:e052602db102 97 ina_val = 0;
taurin 0:e052602db102 98 if(VCmonitor.rawRead(0x00,&ina_val) != 0){
taurin 0:e052602db102 99 pc.printf("VCmonitor READ ERROR\n");
taurin 0:e052602db102 100 return false;
taurin 0:e052602db102 101 }
taurin 0:e052602db102 102 VCmonitor.setCurrentCalibration();
taurin 0:e052602db102 103 return true;
taurin 0:e052602db102 104 }
taurin 0:e052602db102 105
taurin 0:e052602db102 106 void init(){
taurin 15:1db5ee4fe7ce 107 if(!LRstatePin){
taurin 13:5e3b4120dbbf 108 eruronTrim = ERURON_TRIM_INI_L;
taurin 13:5e3b4120dbbf 109 drugTrim = DRUG_TRIM_INI_L;
taurin 13:5e3b4120dbbf 110 eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
taurin 13:5e3b4120dbbf 111 drugMoveDeg = DRUG_MOVE_DEG_INI_L;
taurin 13:5e3b4120dbbf 112 }
taurin 13:5e3b4120dbbf 113 else{
taurin 12:fd9d241843f4 114 eruronTrim = ERURON_TRIM_INI_R;
taurin 12:fd9d241843f4 115 drugTrim = DRUG_TRIM_INI_R;
taurin 12:fd9d241843f4 116 eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
taurin 13:5e3b4120dbbf 117 drugMoveDeg =DRUG_MOVE_DEG_INI_R;
taurin 12:fd9d241843f4 118 }
taurin 0:e052602db102 119 SERVO_FLAG = servoInit();
tsumagari 22:b38bc18ec3a1 120 // ADXL_FLAG = adxlInit();
tsumagari 22:b38bc18ec3a1 121 MPU_FLAG = mpu.testConnection();
taurin 0:e052602db102 122 INA_FLAG = inaInit();
taurin 4:450cafd95ac3 123 sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
taurin 16:82310bf7c326 124 // toStringTicker.attach(&toString,0.5);
taurin 16:82310bf7c326 125 receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
taurin 0:e052602db102 126 }
taurin 0:e052602db102 127
taurin 0:e052602db102 128 void updateDatas(){
tsumagari 22:b38bc18ec3a1 129 // if(ADXL_FLAG){
tsumagari 22:b38bc18ec3a1 130 //// accelerometer.getOutput(acc);
tsumagari 22:b38bc18ec3a1 131 // }
taurin 0:e052602db102 132 if(INA_FLAG){
taurin 0:e052602db102 133 int tmp = VCmonitor.getVoltage(&V);
taurin 0:e052602db102 134 tmp = VCmonitor.getCurrent(&C);
taurin 0:e052602db102 135 }
tsumagari 22:b38bc18ec3a1 136 if(MPU_FLAG){
tsumagari 22:b38bc18ec3a1 137 mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6);
taurin 0:e052602db102 138 }
tsumagari 22:b38bc18ec3a1 139 for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++){
tsumagari 22:b38bc18ec3a1 140 // toSendDatas[i] = acc[i];
tsumagari 22:b38bc18ec3a1 141 toSendDatas[i] = gyro_c[i];
tsumagari 22:b38bc18ec3a1 142 }
tsumagari 22:b38bc18ec3a1 143 // toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
tsumagari 22:b38bc18ec3a1 144 toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77;
taurin 0:e052602db102 145 }
taurin 0:e052602db102 146
taurin 0:e052602db102 147 void receiveDatas(){
taurin 0:e052602db102 148 if(can.read(recmsg)){
taurin 2:7fcb4f970a02 149 for(int i = 0; i < CONTROL_VALUES_NUM; i++){
YusukeWakuta 3:4417217b4f66 150 controlValues[i] = recmsg.data[i];
tsumagari 23:d551db88df65 151 if(i<CONTROL_VALUES_NUM-1) floatvalues[i] = controlValues[i];
taurin 0:e052602db102 152 }
tsumagari 23:d551db88df65 153 eruronfloat = *(const float *)floatvalues;
taurin 1:9cc932a16d17 154 led1 = !led1;
taurin 16:82310bf7c326 155 //WriteServo();
taurin 0:e052602db102 156 }
taurin 0:e052602db102 157 }
taurin 0:e052602db102 158
taurin 0:e052602db102 159 void toString(){
taurin 4:450cafd95ac3 160 for(int i = 0; i <CONTROL_VALUES_NUM; i++){
taurin 4:450cafd95ac3 161 pc.printf("%d, ",controlValues[i]);
taurin 4:450cafd95ac3 162 }
taurin 4:450cafd95ac3 163 pc.printf("\n\r");
taurin 0:e052602db102 164 }
taurin 0:e052602db102 165
taurin 1:9cc932a16d17 166 double calcPulse(int deg){
taurin 4:450cafd95ac3 167 return (0.0006+(deg/180.0)*(0.00235-0.00045));
taurin 4:450cafd95ac3 168
taurin 1:9cc932a16d17 169 }
taurin 1:9cc932a16d17 170
taurin 1:9cc932a16d17 171 void WriteServo(){
taurin 12:fd9d241843f4 172 //if(debugServoPin){
taurin 12:fd9d241843f4 173 // led3 = 1;
taurin 12:fd9d241843f4 174 // float a = eruronAna.read()*180;
taurin 12:fd9d241843f4 175 // float b = drugAna.read()*180;
taurin 12:fd9d241843f4 176 // eruronServo.pulsewidth(calcPulse(eruronAna.read()*180));
taurin 12:fd9d241843f4 177 // drugServo.pulsewidth(calcPulse(drugAna.read()*180));
taurin 12:fd9d241843f4 178 // pc.printf("eruron:%f drug:%f\n\r",a,b);
taurin 12:fd9d241843f4 179 // }
taurin 12:fd9d241843f4 180 // else{
taurin 12:fd9d241843f4 181 // led3 = 0;
tsumagari 23:d551db88df65 182 // eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (controlValues[0]-1) ));
tsumagari 23:d551db88df65 183 drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
tsumagari 23:d551db88df65 184 eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronfloat-1) ));
taurin 12:fd9d241843f4 185 //}
taurin 4:450cafd95ac3 186 }
taurin 4:450cafd95ac3 187
taurin 4:450cafd95ac3 188 void setTrim(){
taurin 4:450cafd95ac3 189 debugLED = 1;
taurin 12:fd9d241843f4 190 if(EDstatePin){
taurin 4:450cafd95ac3 191 eruronTrim = eruronAna.read()*180;
taurin 12:fd9d241843f4 192 eruronServo.pulsewidth(calcPulse(eruronTrim));
taurin 12:fd9d241843f4 193 }
taurin 12:fd9d241843f4 194 else{
taurin 4:450cafd95ac3 195 drugTrim = drugAna.read()*180;
taurin 4:450cafd95ac3 196 drugServo.pulsewidth(calcPulse(drugTrim));
taurin 12:fd9d241843f4 197 }
YusukeWakuta 14:1f6dd929d7de 198 //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim);
YusukeWakuta 14:1f6dd929d7de 199 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
tsumagari 23:d551db88df65 200 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 201 }
taurin 4:450cafd95ac3 202
taurin 4:450cafd95ac3 203 void checkMaxDeg(){
taurin 4:450cafd95ac3 204 led4 = 1;
taurin 4:450cafd95ac3 205 float eruronTemp = eruronAna.read()*180;
taurin 4:450cafd95ac3 206 float drugTemp = drugAna.read()*180;
taurin 12:fd9d241843f4 207 if(EDstatePin){
taurin 4:450cafd95ac3 208 eruronServo.pulsewidth(calcPulse(eruronTemp));
taurin 12:fd9d241843f4 209 eruronMoveDeg = eruronTemp-eruronTrim;
taurin 12:fd9d241843f4 210 }
taurin 12:fd9d241843f4 211 else{
taurin 4:450cafd95ac3 212 drugServo.pulsewidth(calcPulse(drugTemp));
taurin 4:450cafd95ac3 213 drugMoveDeg = drugTemp-drugTrim;
taurin 12:fd9d241843f4 214 }
YusukeWakuta 14:1f6dd929d7de 215 // pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
YusukeWakuta 14:1f6dd929d7de 216 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
tsumagari 23:d551db88df65 217 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 218 wait_us(10);
taurin 1:9cc932a16d17 219 }
taurin 0:e052602db102 220
taurin 0:e052602db102 221 int main(){
taurin 0:e052602db102 222 init();
taurin 0:e052602db102 223 while(1){
taurin 4:450cafd95ac3 224 while(setTrimPin){
taurin 4:450cafd95ac3 225 setTrim();
taurin 4:450cafd95ac3 226 }
taurin 4:450cafd95ac3 227 while(checkMaxDegPin){
taurin 4:450cafd95ac3 228 checkMaxDeg();
taurin 4:450cafd95ac3 229 }
YusukeWakuta 14:1f6dd929d7de 230 // pc.printf("eT:%f\n\r",eruronTrim);
YusukeWakuta 14:1f6dd929d7de 231 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
tsumagari 23:d551db88df65 232 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 233 led4 = 0;
YusukeWakuta 14:1f6dd929d7de 234
taurin 4:450cafd95ac3 235 debugLED = 0;
taurin 16:82310bf7c326 236 //receiveDatas();
tsumagari 22:b38bc18ec3a1 237 // sendDatas();
taurin 1:9cc932a16d17 238 WriteServo();
taurin 4:450cafd95ac3 239 updateDatas();
tsumagari 22:b38bc18ec3a1 240 // pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]);
tsumagari 22:b38bc18ec3a1 241 led3 = !led3;
taurin 2:7fcb4f970a02 242 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 243 }
taurin 0:e052602db102 244 }