ikarashiMDCslave_2byte - ikarashiMDCslaveの改良(?)版　スピード制御の分解能が2byte

NagaokaRoboticsClub_mbedTeam » Code » ikarashiMDCslave_2byte

NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated ikarashiMDCslave_2byte

ikarashiMDCslaveの改良(?)版　スピード制御の分解能が2byte

Dependencies: mbed SoftPWM MotorSMLAP

main.cpp@15:0db5eea67306, 2019-09-02 (annotated)

Committer:: skouki
Date:: Mon Sep 02 05:32:28 2019 +0000
Revision:: 15:0db5eea67306
Parent:: 14:f3d19afbb99e
Child:: 16:8c2eb2b4bead

Who changed what in which revision?

User	Revision	Line number	New contents of line
WAT34	0:4791d1cb39f8	1	#include "mbed.h"
WAT34	0:4791d1cb39f8	2	#include "motorsmlap.h"
WAT34	4:2fb782334564	3	#include "MDC3_0pinConfig.h"
takeuchi	10:5f932239b8a5	4	#include "SoftPWM.h"
Wataru Nakata	7:5ebe1058ca5b	5	#include <vector>
WAT34	6:eccd0b81ba62	6	#define TIMEOUT 0.5
WAT34	4:2fb782334564	7	using namespace pinConfig;
WAT34	9:1579e5cfa499	8	motorSmLap motor[]= {
WAT34	4:2fb782334564	9	motorSmLap(DIR_H_0,DIR_L_0,PWM0),
WAT34	4:2fb782334564	10	motorSmLap(DIR_H_1,DIR_L_1,PWM1),
WAT34	4:2fb782334564	11	motorSmLap(DIR_H_2,DIR_L_2,PWM2),
WAT34	4:2fb782334564	12	motorSmLap(DIR_H_3,DIR_L_3,PWM3)
WAT34	4:2fb782334564	13	};
WAT34	4:2fb782334564	14	Serial rs485(RS485_TX,RS485_RX);
WAT34	4:2fb782334564	15	Serial serial(UART1_TX,UART1_RX);
WAT34	11:3ef351087c94	16	CAN can(CAN_RX, CAN_TX);
WAT34	4:2fb782334564	17
WAT34	4:2fb782334564	18	DigitalOut RSControl(RS485_CS);
Wataru Nakata	7:5ebe1058ca5b	19	bool addrChecked;
Wataru Nakata	7:5ebe1058ca5b	20	bool headerRecieved;
WAT34	13:0cd5e8c91941	21	DigitalIn test1(pinConfig::I2C_SDA);
WAT34	13:0cd5e8c91941	22	DigitalIn test2(pinConfig::I2C_SCL);
WAT34	9:1579e5cfa499	23	DigitalOut debugLED0(LED_0);
WAT34	9:1579e5cfa499	24	DigitalOut debugLED1(LED_1);
WAT34	5:bcf70ca27a55	25	BusOut debugLED(LED_2,LED_3);
WAT34	0:4791d1cb39f8	26	Timeout timeout;
WAT34	0:4791d1cb39f8	27	uint8_t pointedMotor;
WAT34	3:22c5eb74817f	28	bool estop =false,doubleHeader = false;
WAT34	5:bcf70ca27a55	29	BusIn addr(DIP_0,DIP_1,DIP_2);
WAT34	5:bcf70ca27a55	30	//uint8_t addr = 2;
takeuchi	10:5f932239b8a5	31	SoftPWM beep(BUZER);
WAT34	5:bcf70ca27a55	32	bool brakeing[4] = {false};
WAT34	11:3ef351087c94	33	bool canRecieved = false;
WAT34	3:22c5eb74817f	34
WAT34	4:2fb782334564	35	int mode[4] = {0};
WAT34	0:4791d1cb39f8	36
Wataru Nakata	7:5ebe1058ca5b	37	std::vector<unsigned char> buf;
skouki	15:0db5eea67306	38	uint8_t canbuf[4];
WAT34	11:3ef351087c94	39	CANMessage msg;
WAT34	13:0cd5e8c91941	40	bool testing = false;
skouki	14:f3d19afbb99e	41	void start();
skouki	14:f3d19afbb99e	42
WAT34	13:0cd5e8c91941	43	void runTest()
WAT34	13:0cd5e8c91941	44	{
WAT34	13:0cd5e8c91941	45	testing = true;
WAT34	13:0cd5e8c91941	46	//beep = 1;
WAT34	13:0cd5e8c91941	47	for(int i = 0;i < 4;i++)
WAT34	13:0cd5e8c91941	48	{
WAT34	13:0cd5e8c91941	49	motor[i].setFreq(1600*i);
WAT34	13:0cd5e8c91941	50	motor[i].setMotorSpeed(0.1);
WAT34	13:0cd5e8c91941	51	wait(0.3);
WAT34	13:0cd5e8c91941	52	motor[i].setMotorSpeed(0);
WAT34	13:0cd5e8c91941	53	motor[i].setFreq(20000.0);
WAT34	13:0cd5e8c91941	54	}
WAT34	13:0cd5e8c91941	55	testing = false;
WAT34	13:0cd5e8c91941	56	//beep = 0;
WAT34	13:0cd5e8c91941	57	}
Wataru Nakata	7:5ebe1058ca5b	58
WAT34	11:3ef351087c94	59	void canRxIt()
WAT34	11:3ef351087c94	60	{
tknara	12:3a734542a2b3	61	if(can.read(msg)&&(msg.id==addr))
tknara	12:3a734542a2b3	62	{
WAT34	11:3ef351087c94	63	canbuf[0] = msg.data[0];
WAT34	11:3ef351087c94	64	canbuf[1] = msg.data[1];
WAT34	11:3ef351087c94	65	canbuf[2] = msg.data[2];
skouki	15:0db5eea67306	66	canbuf[3] = msg.data[3];
WAT34	11:3ef351087c94	67	canRecieved = true;
WAT34	11:3ef351087c94	68	}
WAT34	11:3ef351087c94	69	}
Wataru Nakata	7:5ebe1058ca5b	70
WAT34	0:4791d1cb39f8	71	void forceStop()
WAT34	0:4791d1cb39f8	72	{
WAT34	13:0cd5e8c91941	73	if(testing)
WAT34	13:0cd5e8c91941	74	return;
WAT34	6:eccd0b81ba62	75	for(int i= 0; i< 4; i++)
WAT34	0:4791d1cb39f8	76	motor[i].setMotorSpeed(0);
WAT34	6:eccd0b81ba62	77	estop = true;
skouki	14:f3d19afbb99e	78
WAT34	0:4791d1cb39f8	79	}
WAT34	2:de85eb142f32	80
WAT34	9:1579e5cfa499	81	void callback()
WAT34	9:1579e5cfa499	82	{
Wataru Nakata	7:5ebe1058ca5b	83	buf.push_back(rs485.getc());
Wataru Nakata	7:5ebe1058ca5b	84	}
Wataru Nakata	7:5ebe1058ca5b	85
WAT34	9:1579e5cfa499	86	void processData(unsigned char data[])
Wataru Nakata	7:5ebe1058ca5b	87	{
skouki	15:0db5eea67306	88	if(data[0]^data[1]^data[2] == data[3]) {
WAT34	9:1579e5cfa499	89	if((data[0]>>5) == addr) {
WAT34	0:4791d1cb39f8	90	addrChecked =true;
WAT34	9:1579e5cfa499	91	pointedMotor = data[0]%4;
WAT34	9:1579e5cfa499	92	mode[pointedMotor] = ((data[0]>>4)%2);
WAT34	9:1579e5cfa499	93	motor[pointedMotor].braking = (data[0]>>3)%2;
Wataru Nakata	7:5ebe1058ca5b	94
WAT34	9:1579e5cfa499	95	motor[pointedMotor].setMode(mode[pointedMotor]);
skouki	15:0db5eea67306	96	if((data[1] + (data[2]<<8)) == 32768) {
WAT34	9:1579e5cfa499	97	motor[pointedMotor].setMotorSpeed(0);
WAT34	9:1579e5cfa499	98	//serial.printf("STOP");
WAT34	9:1579e5cfa499	99	} else {
skouki	15:0db5eea67306	100	motor[pointedMotor].setMotorSpeed(((data[1] + (data[2]<<8))-32768.0)/32768.0);
WAT34	9:1579e5cfa499	101	}
WAT34	9:1579e5cfa499	102	addrChecked = false;
WAT34	4:2fb782334564	103	headerRecieved = false;
WAT34	9:1579e5cfa499	104	serial.putc(pointedMotor);
WAT34	9:1579e5cfa499	105	timeout.attach(&forceStop,TIMEOUT);
WAT34	5:bcf70ca27a55	106	}
WAT34	0:4791d1cb39f8	107	}
WAT34	0:4791d1cb39f8	108	}
WAT34	0:4791d1cb39f8	109
WAT34	9:1579e5cfa499	110
skouki	14:f3d19afbb99e	111	void start()
WAT34	9:1579e5cfa499	112	{
WAT34	6:eccd0b81ba62	113	for(int i= 0; i< 4; i++)
WAT34	13:0cd5e8c91941	114	{
WAT34	13:0cd5e8c91941	115	motor[i].setMode(SM);
WAT34	6:eccd0b81ba62	116	motor[i].setMotorSpeed(0);
WAT34	13:0cd5e8c91941	117	}
skouki	15:0db5eea67306	118	beep.period(1.0 / 3951.066);
skouki	15:0db5eea67306	119	beep = 0.5;
skouki	14:f3d19afbb99e	120	wait(0.4);
skouki	14:f3d19afbb99e	121
skouki	14:f3d19afbb99e	122	beep = 0.0;
takeuchi	10:5f932239b8a5	123	wait(0.1);
skouki	14:f3d19afbb99e	124
skouki	15:0db5eea67306	125	beep.period(1.0 / 3951.066);
skouki	15:0db5eea67306	126	beep = 0.5;
skouki	15:0db5eea67306	127	wait(0.2);
skouki	15:0db5eea67306	128
skouki	15:0db5eea67306	129	beep = 0.0;
skouki	15:0db5eea67306	130	wait(0.1);
skouki	15:0db5eea67306	131
skouki	15:0db5eea67306	132	beep.period(1.0 / 3951.066);
skouki	15:0db5eea67306	133	beep = 0.5;
skouki	15:0db5eea67306	134	wait(0.2);
skouki	14:f3d19afbb99e	135
takeuchi	10:5f932239b8a5	136	beep = 0.0;
takeuchi	10:5f932239b8a5	137	wait(0.1);
skouki	14:f3d19afbb99e	138
skouki	15:0db5eea67306	139	beep.period(1.0 / 3951.066);
skouki	15:0db5eea67306	140	beep = 0.5;
skouki	15:0db5eea67306	141	wait(0.2);
skouki	15:0db5eea67306	142
skouki	15:0db5eea67306	143	beep = 0.0;
skouki	15:0db5eea67306	144	wait(0.1);
skouki	15:0db5eea67306	145
skouki	15:0db5eea67306	146	beep.period(1.0 / 3951.066);
skouki	15:0db5eea67306	147	beep = 0.5;
skouki	15:0db5eea67306	148	wait(0.4);
skouki	15:0db5eea67306	149
skouki	15:0db5eea67306	150	beep = 0.0;
skouki	15:0db5eea67306	151	wait(0.1);
skouki	15:0db5eea67306	152
skouki	15:0db5eea67306	153	beep.period(1.0 / 2637.020);
takeuchi	10:5f932239b8a5	154	for(int i = 0; i<addr; i++) {
skouki	15:0db5eea67306	155	beep = 0.1;
skouki	15:0db5eea67306	156	wait(0.2);
takeuchi	10:5f932239b8a5	157	beep = 0.0;
takeuchi	10:5f932239b8a5	158	wait(0.075);
skouki	14:f3d19afbb99e	159	} // beep addr times
skouki	14:f3d19afbb99e	160	beep.period(1.0 / 1000.0);
skouki	14:f3d19afbb99e	161
skouki	14:f3d19afbb99e	162	}
skouki	14:f3d19afbb99e	163	int main()
skouki	14:f3d19afbb99e	164	{
skouki	14:f3d19afbb99e	165	start();
WAT34	9:1579e5cfa499	166
skouki	15:0db5eea67306	167	unsigned char tmp[4] = {0};
eil4nyqn	1:d802a9793080	168	addrChecked=false,headerRecieved=false;
WAT34	11:3ef351087c94	169	can.attach(&canRxIt, CAN::RxIrq);
WAT34	4:2fb782334564	170	//motor[0].setMotorSpeed(0);
takeuchi	10:5f932239b8a5	171	rs485.baud(115200);
eil4nyqn	1:d802a9793080	172	serial.baud(115200);
WAT34	5:bcf70ca27a55	173	addr.mode(PullUp);
WAT34	13:0cd5e8c91941	174	test1.mode(PullDown);
WAT34	13:0cd5e8c91941	175	test2.mode(PullDown);
WAT34	4:2fb782334564	176	RSControl = 0;
WAT34	4:2fb782334564	177	rs485.putc((1<<addr));
WAT34	4:2fb782334564	178	rs485.attach(&callback);
takeuchi	10:5f932239b8a5	179	beep = 0.0;
WAT34	5:bcf70ca27a55	180	while(1) {
Wataru Nakata	7:5ebe1058ca5b	181	debugLED = addr;
skouki	15:0db5eea67306	182	if(buf.size() > 5) {
WAT34	9:1579e5cfa499	183	if (buf[0] == 255) {
WAT34	9:1579e5cfa499	184	tmp[0] = buf[1];
WAT34	9:1579e5cfa499	185	tmp[1] = buf[2];
WAT34	9:1579e5cfa499	186	tmp[2] = buf[3];
skouki	15:0db5eea67306	187	tmp[3] = buf[4];
WAT34	9:1579e5cfa499	188	processData(tmp);
skouki	15:0db5eea67306	189	buf.erase(buf.begin(),buf.begin()+4);
takeuchi	10:5f932239b8a5	190	debugLED0 = !debugLED0;
WAT34	9:1579e5cfa499	191	} else {
WAT34	9:1579e5cfa499	192	buf.erase(buf.begin());
takeuchi	10:5f932239b8a5	193	debugLED0 = false;
WAT34	9:1579e5cfa499	194	}
Wataru Nakata	7:5ebe1058ca5b	195	}
WAT34	11:3ef351087c94	196	if(canRecieved)
WAT34	11:3ef351087c94	197	{
WAT34	11:3ef351087c94	198	tmp[0] = canbuf[0];
WAT34	11:3ef351087c94	199	tmp[1] = canbuf[1];
WAT34	11:3ef351087c94	200	tmp[2] = canbuf[2];
skouki	15:0db5eea67306	201	tmp[3] = canbuf[3];
WAT34	11:3ef351087c94	202	processData(tmp);
WAT34	11:3ef351087c94	203	debugLED0 = !debugLED0;
WAT34	11:3ef351087c94	204	canRecieved = false;
WAT34	11:3ef351087c94	205	}
WAT34	2:de85eb142f32	206	if(estop)
WAT34	2:de85eb142f32	207	forceStop();
WAT34	2:de85eb142f32	208	beep = estop;
WAT34	13:0cd5e8c91941	209
WAT34	13:0cd5e8c91941	210	if(test1 && test2)
WAT34	13:0cd5e8c91941	211	runTest();
WAT34	0:4791d1cb39f8	212	}
WAT34	0:4791d1cb39f8	213	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	30 Aug 2019
Imports:	12
Forks:	0
Commits:	19
Dependents:	0
Dependencies:	3
Followers:	57

main.cpp@15:0db5eea67306, 2019-09-02 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning