RTno_MotorControl -

Users » nucho » Code » RTno_MotorControl

nucho / Mbed 2 deprecated RTno_MotorControl

main.cpp@1:7f0fc0d1f777, 2011-08-01 (annotated)

Committer:: nucho
Date:: Mon Aug 01 14:46:08 2011 +0000
Revision:: 1:7f0fc0d1f777
Parent:: 0:3c49891bc39d

Who changed what in which revision?

User	Revision	Line number	New contents of line
nucho	0:3c49891bc39d	1	#include "mbed.h"
nucho	0:3c49891bc39d	2	#include "RTno.h"
nucho	0:3c49891bc39d	3
nucho	0:3c49891bc39d	4	#include "QEI.h"
nucho	0:3c49891bc39d	5	#include "SimplePID.h"
nucho	0:3c49891bc39d	6	#define MOTOR_OFFSET 1460
nucho	0:3c49891bc39d	7	#define KP 5.1
nucho	0:3c49891bc39d	8	#define KI 0.0
nucho	0:3c49891bc39d	9	#define KD 0.0
nucho	0:3c49891bc39d	10	#define RATE 0.2
nucho	0:3c49891bc39d	11
nucho	0:3c49891bc39d	12	PwmOut motor1(p21);
nucho	0:3c49891bc39d	13	QEI qei_motor1(p29, p30, NC, 624);
nucho	0:3c49891bc39d	14	SimplePID pid_motor1(KP,KI,KD,RATE);
nucho	0:3c49891bc39d	15
nucho	0:3c49891bc39d	16	PwmOut motor2(p22);
nucho	0:3c49891bc39d	17	QEI qei_motor2(p27, p28, NC, 624);
nucho	0:3c49891bc39d	18	SimplePID pid_motor2(KP,KI,KD,RATE);
nucho	0:3c49891bc39d	19	/**
nucho	0:3c49891bc39d	20	* digitalInOut.pde
nucho	0:3c49891bc39d	21	* RTno is RT-middleware and arduino.
nucho	0:3c49891bc39d	22	*
nucho	0:3c49891bc39d	23	* Using RTno, arduino device can communicate any RT-components
nucho	0:3c49891bc39d	24	* through the RTno-proxy component which is launched in PC.
nucho	0:3c49891bc39d	25	* Connect arduino with USB, and program with RTno library.
nucho	0:3c49891bc39d	26	* You do not have to define any protocols to establish communication
nucho	0:3c49891bc39d	27	* between arduino and PC.
nucho	0:3c49891bc39d	28	*
nucho	0:3c49891bc39d	29	* Using RTno, you must not define the function "setup" and "loop".
nucho	0:3c49891bc39d	30	* Those functions are automatically defined in the RTno libarary.
nucho	0:3c49891bc39d	31	* You, developers, must define following functions:
nucho	0:3c49891bc39d	32	* int onInitialize(void);
nucho	0:3c49891bc39d	33	* int onActivated(void);
nucho	0:3c49891bc39d	34	* int onDeactivated(void);
nucho	0:3c49891bc39d	35	* int onExecute(void);
nucho	0:3c49891bc39d	36	* int onError(void);
nucho	0:3c49891bc39d	37	* int onReset(void);
nucho	0:3c49891bc39d	38	* These functions are spontaneously called by the RTno-proxy
nucho	0:3c49891bc39d	39	* RT-component which is launched in the PC.
nucho	0:3c49891bc39d	40	*/
nucho	0:3c49891bc39d	41
nucho	0:3c49891bc39d	42
nucho	0:3c49891bc39d	43	/**
nucho	0:3c49891bc39d	44	* This function is called at first.
nucho	0:3c49891bc39d	45	* conf._default.baudrate: baudrate of serial communication
nucho	0:3c49891bc39d	46	* exec_cxt.periodic.type: reserved but not used.
nucho	0:3c49891bc39d	47	*/
nucho	0:3c49891bc39d	48	void rtcconf(void) {
nucho	0:3c49891bc39d	49	conf._default.baudrate = 115200;
nucho	0:3c49891bc39d	50	exec_cxt.periodic.type = ProxySynchronousExecutionContext;
nucho	0:3c49891bc39d	51	}
nucho	0:3c49891bc39d	52
nucho	0:3c49891bc39d	53	/**
nucho	0:3c49891bc39d	54	* Declaration Division:
nucho	0:3c49891bc39d	55	*
nucho	0:3c49891bc39d	56	* DataPort and Data Buffer should be placed here.
nucho	0:3c49891bc39d	57	*
nucho	0:3c49891bc39d	58	* Currently, following 6 types are available.
nucho	0:3c49891bc39d	59	* TimedLong:
nucho	0:3c49891bc39d	60	* TimedDouble:
nucho	0:3c49891bc39d	61	* TimedFloat:
nucho	0:3c49891bc39d	62	* TimedLongSeq:
nucho	0:3c49891bc39d	63	* TimedDoubleSeq:
nucho	0:3c49891bc39d	64	* TimedFloatSeq:
nucho	0:3c49891bc39d	65	*
nucho	0:3c49891bc39d	66	* Please refer following comments. If you need to use some ports,
nucho	0:3c49891bc39d	67	* uncomment the line you want to declare.
nucho	0:3c49891bc39d	68	**/
nucho	0:3c49891bc39d	69	TimedLongSeq position;
nucho	0:3c49891bc39d	70	InPort positionIn("position", position);
nucho	0:3c49891bc39d	71
nucho	0:3c49891bc39d	72	TimedLongSeq encorder;
nucho	0:3c49891bc39d	73	OutPort encorderOut("encorder", encorder);
nucho	0:3c49891bc39d	74
nucho	0:3c49891bc39d	75
nucho	1:7f0fc0d1f777	76
nucho	0:3c49891bc39d	77	//////////////////////////////////////////
nucho	0:3c49891bc39d	78	// on_initialize
nucho	0:3c49891bc39d	79	//
nucho	0:3c49891bc39d	80	// This function is called in the initialization
nucho	0:3c49891bc39d	81	// sequence. The sequence is triggered by the
nucho	0:3c49891bc39d	82	// PC. When the RTnoRTC is launched in the PC,
nucho	0:3c49891bc39d	83	// then, this function is remotely called
nucho	0:3c49891bc39d	84	// through the USB cable.
nucho	0:3c49891bc39d	85	// In on_initialize, usually DataPorts are added.
nucho	0:3c49891bc39d	86	//
nucho	0:3c49891bc39d	87	//////////////////////////////////////////
nucho	0:3c49891bc39d	88	int RTno::onInitialize() {
nucho	0:3c49891bc39d	89	/* Data Ports are added in this section.
nucho	0:3c49891bc39d	90	*/
nucho	0:3c49891bc39d	91	addInPort(positionIn);
nucho	0:3c49891bc39d	92	addOutPort(encorderOut);
nucho	0:3c49891bc39d	93
nucho	0:3c49891bc39d	94	// Some initialization (like port direction setting)
nucho	0:3c49891bc39d	95
nucho	0:3c49891bc39d	96	return RTC_OK;
nucho	0:3c49891bc39d	97	}
nucho	0:3c49891bc39d	98
nucho	0:3c49891bc39d	99	////////////////////////////////////////////
nucho	0:3c49891bc39d	100	// on_activated
nucho	0:3c49891bc39d	101	// This function is called when the RTnoRTC
nucho	0:3c49891bc39d	102	// is activated. When the activation, the RTnoRTC
nucho	0:3c49891bc39d	103	// sends message to call this function remotely.
nucho	0:3c49891bc39d	104	// If this function is failed (return value
nucho	0:3c49891bc39d	105	// is RTC_ERROR), RTno will enter ERROR condition.
nucho	0:3c49891bc39d	106	////////////////////////////////////////////
nucho	0:3c49891bc39d	107	int RTno::onActivated() {
nucho	0:3c49891bc39d	108	// Write here initialization code.
nucho	0:3c49891bc39d	109
nucho	0:3c49891bc39d	110	return RTC_OK;
nucho	0:3c49891bc39d	111	}
nucho	0:3c49891bc39d	112
nucho	0:3c49891bc39d	113	/////////////////////////////////////////////
nucho	0:3c49891bc39d	114	// on_deactivated
nucho	0:3c49891bc39d	115	// This function is called when the RTnoRTC
nucho	0:3c49891bc39d	116	// is deactivated.
nucho	0:3c49891bc39d	117	/////////////////////////////////////////////
nucho	0:3c49891bc39d	118	int RTno::onDeactivated() {
nucho	0:3c49891bc39d	119	// Write here finalization code.
nucho	0:3c49891bc39d	120
nucho	0:3c49891bc39d	121	return RTC_OK;
nucho	0:3c49891bc39d	122	}
nucho	0:3c49891bc39d	123
nucho	0:3c49891bc39d	124	//////////////////////////////////////////////
nucho	0:3c49891bc39d	125	// This function is repeatedly called when the
nucho	0:3c49891bc39d	126	// RTno is in the ACTIVE condition.
nucho	0:3c49891bc39d	127	// If this function is failed (return value is
nucho	0:3c49891bc39d	128	// RTC_ERROR), RTno immediately enter into the
nucho	0:3c49891bc39d	129	// ERROR condition.r
nucho	0:3c49891bc39d	130	//////////////////////////////////////////////
nucho	0:3c49891bc39d	131	int RTno::onExecute() {
nucho	0:3c49891bc39d	132
nucho	0:3c49891bc39d	133	/*
nucho	0:3c49891bc39d	134	* Input
nucho	0:3c49891bc39d	135	*/
nucho	0:3c49891bc39d	136	if (positionIn.isNew()) {
nucho	0:3c49891bc39d	137	positionIn.read();
nucho	0:3c49891bc39d	138	pid_motor1.setGoal(position.data[0]);
nucho	0:3c49891bc39d	139	pid_motor2.setGoal(position.data[1]);
nucho	0:3c49891bc39d	140	}
nucho	0:3c49891bc39d	141
nucho	0:3c49891bc39d	142	pid_motor1.setLimits(-500,500);
nucho	0:3c49891bc39d	143	pid_motor2.setLimits(-500,500);
nucho	0:3c49891bc39d	144
nucho	0:3c49891bc39d	145	/*
nucho	0:3c49891bc39d	146	* Output
nucho	0:3c49891bc39d	147	*/
nucho	0:3c49891bc39d	148	int current1,current2;
nucho	0:3c49891bc39d	149	current1 = qei_motor1.getPulses();
nucho	0:3c49891bc39d	150	current2 = qei_motor2.getPulses();
nucho	0:3c49891bc39d	151
nucho	0:3c49891bc39d	152	encorder.data.length(2);
nucho	0:3c49891bc39d	153	encorder.data[0] = current1;
nucho	0:3c49891bc39d	154	encorder.data[1] = current2;
nucho	0:3c49891bc39d	155	encorderOut.write();
nucho	0:3c49891bc39d	156
nucho	0:3c49891bc39d	157	int ctrl1,ctrl2;
nucho	0:3c49891bc39d	158	ctrl1 = pid_motor1.compute(current1);
nucho	0:3c49891bc39d	159	ctrl2 = pid_motor2.compute(current2);
nucho	0:3c49891bc39d	160
nucho	0:3c49891bc39d	161	motor1.pulsewidth_us(ctrl1+MOTOR_OFFSET);
nucho	0:3c49891bc39d	162	motor2.pulsewidth_us(ctrl2+MOTOR_OFFSET);
nucho	0:3c49891bc39d	163
nucho	0:3c49891bc39d	164	return RTC_OK;
nucho	0:3c49891bc39d	165	}
nucho	0:3c49891bc39d	166
nucho	0:3c49891bc39d	167
nucho	0:3c49891bc39d	168	//////////////////////////////////////
nucho	0:3c49891bc39d	169	// on_error
nucho	0:3c49891bc39d	170	// This function is repeatedly called when
nucho	0:3c49891bc39d	171	// the RTno is in the ERROR condition.
nucho	0:3c49891bc39d	172	// The ERROR condition can be recovered,
nucho	0:3c49891bc39d	173	// when the RTno is reset.
nucho	0:3c49891bc39d	174	///////////////////////////////////////
nucho	0:3c49891bc39d	175	int RTno::onError() {
nucho	0:3c49891bc39d	176	return RTC_OK;
nucho	0:3c49891bc39d	177	}
nucho	0:3c49891bc39d	178
nucho	0:3c49891bc39d	179	////////////////////////////////////////
nucho	0:3c49891bc39d	180	// This function is called when
nucho	0:3c49891bc39d	181	// the RTno is reset. If on_reset is
nucho	0:3c49891bc39d	182	// succeeded, the RTno will enter into
nucho	0:3c49891bc39d	183	// the INACTIVE condition. If failed
nucho	0:3c49891bc39d	184	// (return value is RTC_ERROR), RTno
nucho	0:3c49891bc39d	185	// will stay in ERROR condition.ec
nucho	0:3c49891bc39d	186	///////////////////////////////////////
nucho	0:3c49891bc39d	187	int RTno::onReset() {
nucho	0:3c49891bc39d	188	return RTC_OK;
nucho	0:3c49891bc39d	189	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	29 Jul 2011
Imports:	4
Forks:	0
Commits:	2
Dependents:	0
Dependencies:	2
Followers:	1

main.cpp@1:7f0fc0d1f777, 2011-08-01 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning