ControlYokutan2018 - 2018年度用翼端mbedプログラム

albatross » Code » ControlYokutan2018

albatross / Mbed 2 deprecated ControlYokutan2018

2018年度用翼端mbedプログラム

Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017_2 by albatross

main.cpp@22:b38bc18ec3a1, 2017-01-28 (annotated)

Committer:: tsumagari
Date:: Sat Jan 28 02:28:06 2017 +0000
Revision:: 22:b38bc18ec3a1
Parent:: 21:8a621c81507d
Child:: 23:d551db88df65
Child:: 24:d416722b4aad

Who changed what in which revision?

User	Revision	Line number	New 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
taurin	0:e052602db102	10	#define CONTROL_VALUES_NUM 2
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
YusukeWakuta	21:8a621c81507d	16	#define ERURON_MOVE_DEG_INI_R 10 // もともと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
YusukeWakuta	21:8a621c81507d	21	#define ERURON_MOVE_DEG_INI_L -8 //もともと-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];
taurin	1:9cc932a16d17	50	char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug
taurin	0:e052602db102	51
taurin	12:fd9d241843f4	52	float eruronTrim;
taurin	12:fd9d241843f4	53	float drugTrim;
taurin	12:fd9d241843f4	54	float eruronMoveDeg;
taurin	12:fd9d241843f4	55	float drugMoveDeg;
taurin	0:e052602db102	56	unsigned short ina_val;
taurin	0:e052602db102	57	double V,C;
taurin	0:e052602db102	58	bool SERVO_FLAG;
taurin	0:e052602db102	59	bool ADXL_FLAG;
taurin	0:e052602db102	60	bool INA_FLAG;
tsumagari	22:b38bc18ec3a1	61	bool MPU_FLAG;
taurin	0:e052602db102	62
tsumagari	22:b38bc18ec3a1	63	//int16_t acc[3] = {0,0,0};
tsumagari	22:b38bc18ec3a1	64	char gyro_c[6] = {0,0,0,0,0,0};
tsumagari	22:b38bc18ec3a1	65	//char acc_mean[3][ADXL_MEAN_NUM];
tsumagari	22:b38bc18ec3a1	66	//int adxl_mean_counter = 0;
taurin	4:450cafd95ac3	67
taurin	4:450cafd95ac3	68	void toString();
taurin	16:82310bf7c326	69	void receiveDatas();
taurin	16:82310bf7c326	70	void WriteServo();
taurin	0:e052602db102	71
taurin	0:e052602db102	72	bool servoInit(){
taurin	4:450cafd95ac3	73	drugServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin	4:450cafd95ac3	74	eruronServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin	0:e052602db102	75	return true;
taurin	0:e052602db102	76	}
taurin	0:e052602db102	77
tsumagari	22:b38bc18ec3a1	78	//bool adxlInit(){
tsumagari	22:b38bc18ec3a1	79	// accelerometer.setPowerControl(0x00);
tsumagari	22:b38bc18ec3a1	80	// accelerometer.setDataFormatControl(0x0B);
tsumagari	22:b38bc18ec3a1	81	// accelerometer.setDataRate(ADXL345_3200HZ);
tsumagari	22:b38bc18ec3a1	82	// accelerometer.setPowerControl(0x08);
tsumagari	22:b38bc18ec3a1	83	// return true;
tsumagari	22:b38bc18ec3a1	84	//}
taurin	0:e052602db102	85
taurin	4:450cafd95ac3	86	void sendDatas(){
taurin	4:450cafd95ac3	87	if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
taurin	4:450cafd95ac3	88	}
taurin	4:450cafd95ac3	89	}
taurin	4:450cafd95ac3	90
taurin	0:e052602db102	91	bool inaInit(){
taurin	0:e052602db102	92	if(!VCmonitor.isExist()){
taurin	0:e052602db102	93	pc.printf("VCmonitor NOT FOUND\n");
taurin	0:e052602db102	94	return false;
taurin	0:e052602db102	95	}
taurin	0:e052602db102	96	ina_val = 0;
taurin	0:e052602db102	97	if(VCmonitor.rawRead(0x00,&ina_val) != 0){
taurin	0:e052602db102	98	pc.printf("VCmonitor READ ERROR\n");
taurin	0:e052602db102	99	return false;
taurin	0:e052602db102	100	}
taurin	0:e052602db102	101	VCmonitor.setCurrentCalibration();
taurin	0:e052602db102	102	return true;
taurin	0:e052602db102	103	}
taurin	0:e052602db102	104
taurin	0:e052602db102	105	void init(){
taurin	15:1db5ee4fe7ce	106	if(!LRstatePin){
taurin	13:5e3b4120dbbf	107	eruronTrim = ERURON_TRIM_INI_L;
taurin	13:5e3b4120dbbf	108	drugTrim = DRUG_TRIM_INI_L;
taurin	13:5e3b4120dbbf	109	eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
taurin	13:5e3b4120dbbf	110	drugMoveDeg = DRUG_MOVE_DEG_INI_L;
taurin	13:5e3b4120dbbf	111	}
taurin	13:5e3b4120dbbf	112	else{
taurin	12:fd9d241843f4	113	eruronTrim = ERURON_TRIM_INI_R;
taurin	12:fd9d241843f4	114	drugTrim = DRUG_TRIM_INI_R;
taurin	12:fd9d241843f4	115	eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
taurin	13:5e3b4120dbbf	116	drugMoveDeg =DRUG_MOVE_DEG_INI_R;
taurin	12:fd9d241843f4	117	}
taurin	0:e052602db102	118	SERVO_FLAG = servoInit();
tsumagari	22:b38bc18ec3a1	119	// ADXL_FLAG = adxlInit();
tsumagari	22:b38bc18ec3a1	120	MPU_FLAG = mpu.testConnection();
taurin	0:e052602db102	121	INA_FLAG = inaInit();
taurin	4:450cafd95ac3	122	sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
taurin	16:82310bf7c326	123	// toStringTicker.attach(&toString,0.5);
taurin	16:82310bf7c326	124	receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
taurin	0:e052602db102	125	}
taurin	0:e052602db102	126
taurin	0:e052602db102	127	void updateDatas(){
tsumagari	22:b38bc18ec3a1	128	// if(ADXL_FLAG){
tsumagari	22:b38bc18ec3a1	129	//// accelerometer.getOutput(acc);
tsumagari	22:b38bc18ec3a1	130	// }
taurin	0:e052602db102	131	if(INA_FLAG){
taurin	0:e052602db102	132	int tmp = VCmonitor.getVoltage(&V);
taurin	0:e052602db102	133	tmp = VCmonitor.getCurrent(&C);
taurin	0:e052602db102	134	}
tsumagari	22:b38bc18ec3a1	135	if(MPU_FLAG){
tsumagari	22:b38bc18ec3a1	136	mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6);
taurin	0:e052602db102	137	}
tsumagari	22:b38bc18ec3a1	138	for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++){
tsumagari	22:b38bc18ec3a1	139	// toSendDatas[i] = acc[i];
tsumagari	22:b38bc18ec3a1	140	toSendDatas[i] = gyro_c[i];
tsumagari	22:b38bc18ec3a1	141	}
tsumagari	22:b38bc18ec3a1	142	// toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
tsumagari	22:b38bc18ec3a1	143	toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77;
taurin	0:e052602db102	144	}
taurin	0:e052602db102	145
taurin	0:e052602db102	146	void receiveDatas(){
taurin	0:e052602db102	147	if(can.read(recmsg)){
taurin	2:7fcb4f970a02	148	for(int i = 0; i < CONTROL_VALUES_NUM; i++){
YusukeWakuta	3:4417217b4f66	149	controlValues[i] = recmsg.data[i];
taurin	0:e052602db102	150	}
taurin	1:9cc932a16d17	151	led1 = !led1;
taurin	16:82310bf7c326	152	//WriteServo();
taurin	0:e052602db102	153	}
taurin	0:e052602db102	154	}
taurin	0:e052602db102	155
taurin	0:e052602db102	156	void toString(){
taurin	4:450cafd95ac3	157	for(int i = 0; i <CONTROL_VALUES_NUM; i++){
taurin	4:450cafd95ac3	158	pc.printf("%d, ",controlValues[i]);
taurin	4:450cafd95ac3	159	}
taurin	4:450cafd95ac3	160	pc.printf("\n\r");
taurin	0:e052602db102	161	}
taurin	0:e052602db102	162
taurin	1:9cc932a16d17	163	double calcPulse(int deg){
taurin	4:450cafd95ac3	164	return (0.0006+(deg/180.0)*(0.00235-0.00045));
taurin	4:450cafd95ac3	165
taurin	1:9cc932a16d17	166	}
taurin	1:9cc932a16d17	167
taurin	1:9cc932a16d17	168	void WriteServo(){
taurin	12:fd9d241843f4	169	//if(debugServoPin){
taurin	12:fd9d241843f4	170	// led3 = 1;
taurin	12:fd9d241843f4	171	// float a = eruronAna.read()*180;
taurin	12:fd9d241843f4	172	// float b = drugAna.read()*180;
taurin	12:fd9d241843f4	173	// eruronServo.pulsewidth(calcPulse(eruronAna.read()*180));
taurin	12:fd9d241843f4	174	// drugServo.pulsewidth(calcPulse(drugAna.read()*180));
taurin	12:fd9d241843f4	175	// pc.printf("eruron:%f drug:%f\n\r",a,b);
taurin	12:fd9d241843f4	176	// }
taurin	12:fd9d241843f4	177	// else{
taurin	12:fd9d241843f4	178	// led3 = 0;
taurin	4:450cafd95ac3	179	eruronServo.pulsewidth(calcPulse(eruronTrim+eruronMoveDeg*(controlValues[0]-1)));
taurin	4:450cafd95ac3	180	drugServo.pulsewidth(calcPulse(drugTrim+drugMoveDeg*controlValues[1]));
taurin	12:fd9d241843f4	181	//}
taurin	4:450cafd95ac3	182	}
taurin	4:450cafd95ac3	183
taurin	4:450cafd95ac3	184	void setTrim(){
taurin	4:450cafd95ac3	185	debugLED = 1;
taurin	12:fd9d241843f4	186	if(EDstatePin){
taurin	4:450cafd95ac3	187	eruronTrim = eruronAna.read()*180;
taurin	12:fd9d241843f4	188	eruronServo.pulsewidth(calcPulse(eruronTrim));
taurin	12:fd9d241843f4	189	}
taurin	12:fd9d241843f4	190	else{
taurin	4:450cafd95ac3	191	drugTrim = drugAna.read()*180;
taurin	4:450cafd95ac3	192	drugServo.pulsewidth(calcPulse(drugTrim));
taurin	12:fd9d241843f4	193	}
YusukeWakuta	14:1f6dd929d7de	194	//pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim);
YusukeWakuta	14:1f6dd929d7de	195	pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta	14:1f6dd929d7de	196	pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
taurin	4:450cafd95ac3	197	}
taurin	4:450cafd95ac3	198
taurin	4:450cafd95ac3	199	void checkMaxDeg(){
taurin	4:450cafd95ac3	200	led4 = 1;
taurin	4:450cafd95ac3	201	float eruronTemp = eruronAna.read()*180;
taurin	4:450cafd95ac3	202	float drugTemp = drugAna.read()*180;
taurin	12:fd9d241843f4	203	if(EDstatePin){
taurin	4:450cafd95ac3	204	eruronServo.pulsewidth(calcPulse(eruronTemp));
taurin	12:fd9d241843f4	205	eruronMoveDeg = eruronTemp-eruronTrim;
taurin	12:fd9d241843f4	206	}
taurin	12:fd9d241843f4	207	else{
taurin	4:450cafd95ac3	208	drugServo.pulsewidth(calcPulse(drugTemp));
taurin	4:450cafd95ac3	209	drugMoveDeg = drugTemp-drugTrim;
taurin	12:fd9d241843f4	210	}
YusukeWakuta	14:1f6dd929d7de	211	// pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
YusukeWakuta	14:1f6dd929d7de	212	pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta	14:1f6dd929d7de	213	pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
taurin	4:450cafd95ac3	214	wait_us(10);
taurin	1:9cc932a16d17	215	}
taurin	0:e052602db102	216
taurin	0:e052602db102	217	int main(){
taurin	0:e052602db102	218	init();
taurin	0:e052602db102	219	while(1){
taurin	4:450cafd95ac3	220	while(setTrimPin){
taurin	4:450cafd95ac3	221	setTrim();
taurin	4:450cafd95ac3	222	}
taurin	4:450cafd95ac3	223	while(checkMaxDegPin){
taurin	4:450cafd95ac3	224	checkMaxDeg();
taurin	4:450cafd95ac3	225	}
YusukeWakuta	14:1f6dd929d7de	226	// pc.printf("eT:%f\n\r",eruronTrim);
YusukeWakuta	14:1f6dd929d7de	227	pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta	14:1f6dd929d7de	228	pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
taurin	4:450cafd95ac3	229	led4 = 0;
YusukeWakuta	14:1f6dd929d7de	230
taurin	4:450cafd95ac3	231	debugLED = 0;
taurin	16:82310bf7c326	232	//receiveDatas();
tsumagari	22:b38bc18ec3a1	233	// sendDatas();
taurin	1:9cc932a16d17	234	WriteServo();
taurin	4:450cafd95ac3	235	updateDatas();
tsumagari	22:b38bc18ec3a1	236	// pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]);
tsumagari	22:b38bc18ec3a1	237	led3 = !led3;
taurin	2:7fcb4f970a02	238	wait(WAIT_LOOP_TIME);
taurin	0:e052602db102	239	}
taurin	0:e052602db102	240	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	15 Jul 2017
Imports:	3
Forks:	0
Commits:	79
Dependents:	0
Dependencies:	4
Followers:	8

main.cpp@22:b38bc18ec3a1, 2017-01-28 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning