RTno_MotorControl -

Users » nucho » Code » RTno_MotorControl

nucho / Mbed 2 deprecated RTno_MotorControl

main.cpp@0:3c49891bc39d, 2011-07-29 (annotated)

Committer:: nucho
Date:: Fri Jul 29 11:23:44 2011 +0000
Revision:: 0:3c49891bc39d
Child:: 1:7f0fc0d1f777

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	0:3c49891bc39d	76	//////////////////////////////////////////
nucho	0:3c49891bc39d	77	// on_initialize
nucho	0:3c49891bc39d	78	//
nucho	0:3c49891bc39d	79	// This function is called in the initialization
nucho	0:3c49891bc39d	80	// sequence. The sequence is triggered by the
nucho	0:3c49891bc39d	81	// PC. When the RTnoRTC is launched in the PC,
nucho	0:3c49891bc39d	82	// then, this function is remotely called
nucho	0:3c49891bc39d	83	// through the USB cable.
nucho	0:3c49891bc39d	84	// In on_initialize, usually DataPorts are added.
nucho	0:3c49891bc39d	85	//
nucho	0:3c49891bc39d	86	//////////////////////////////////////////
nucho	0:3c49891bc39d	87	int RTno::onInitialize() {
nucho	0:3c49891bc39d	88	/* Data Ports are added in this section.
nucho	0:3c49891bc39d	89	*/
nucho	0:3c49891bc39d	90	addInPort(positionIn);
nucho	0:3c49891bc39d	91	addOutPort(encorderOut);
nucho	0:3c49891bc39d	92
nucho	0:3c49891bc39d	93	// Some initialization (like port direction setting)
nucho	0:3c49891bc39d	94
nucho	0:3c49891bc39d	95	return RTC_OK;
nucho	0:3c49891bc39d	96	}
nucho	0:3c49891bc39d	97
nucho	0:3c49891bc39d	98	////////////////////////////////////////////
nucho	0:3c49891bc39d	99	// on_activated
nucho	0:3c49891bc39d	100	// This function is called when the RTnoRTC
nucho	0:3c49891bc39d	101	// is activated. When the activation, the RTnoRTC
nucho	0:3c49891bc39d	102	// sends message to call this function remotely.
nucho	0:3c49891bc39d	103	// If this function is failed (return value
nucho	0:3c49891bc39d	104	// is RTC_ERROR), RTno will enter ERROR condition.
nucho	0:3c49891bc39d	105	////////////////////////////////////////////
nucho	0:3c49891bc39d	106	int RTno::onActivated() {
nucho	0:3c49891bc39d	107	// Write here initialization code.
nucho	0:3c49891bc39d	108
nucho	0:3c49891bc39d	109	return RTC_OK;
nucho	0:3c49891bc39d	110	}
nucho	0:3c49891bc39d	111
nucho	0:3c49891bc39d	112	/////////////////////////////////////////////
nucho	0:3c49891bc39d	113	// on_deactivated
nucho	0:3c49891bc39d	114	// This function is called when the RTnoRTC
nucho	0:3c49891bc39d	115	// is deactivated.
nucho	0:3c49891bc39d	116	/////////////////////////////////////////////
nucho	0:3c49891bc39d	117	int RTno::onDeactivated() {
nucho	0:3c49891bc39d	118	// Write here finalization code.
nucho	0:3c49891bc39d	119
nucho	0:3c49891bc39d	120	return RTC_OK;
nucho	0:3c49891bc39d	121	}
nucho	0:3c49891bc39d	122
nucho	0:3c49891bc39d	123	//////////////////////////////////////////////
nucho	0:3c49891bc39d	124	// This function is repeatedly called when the
nucho	0:3c49891bc39d	125	// RTno is in the ACTIVE condition.
nucho	0:3c49891bc39d	126	// If this function is failed (return value is
nucho	0:3c49891bc39d	127	// RTC_ERROR), RTno immediately enter into the
nucho	0:3c49891bc39d	128	// ERROR condition.r
nucho	0:3c49891bc39d	129	//////////////////////////////////////////////
nucho	0:3c49891bc39d	130	int RTno::onExecute() {
nucho	0:3c49891bc39d	131
nucho	0:3c49891bc39d	132	/*
nucho	0:3c49891bc39d	133	* Input
nucho	0:3c49891bc39d	134	*/
nucho	0:3c49891bc39d	135	if (positionIn.isNew()) {
nucho	0:3c49891bc39d	136	positionIn.read();
nucho	0:3c49891bc39d	137	pid_motor1.setGoal(position.data[0]);
nucho	0:3c49891bc39d	138	pid_motor2.setGoal(position.data[1]);
nucho	0:3c49891bc39d	139	}
nucho	0:3c49891bc39d	140
nucho	0:3c49891bc39d	141	pid_motor1.setLimits(-500,500);
nucho	0:3c49891bc39d	142	pid_motor2.setLimits(-500,500);
nucho	0:3c49891bc39d	143
nucho	0:3c49891bc39d	144	/*
nucho	0:3c49891bc39d	145	* Output
nucho	0:3c49891bc39d	146	*/
nucho	0:3c49891bc39d	147	int current1,current2;
nucho	0:3c49891bc39d	148	current1 = qei_motor1.getPulses();
nucho	0:3c49891bc39d	149	current2 = qei_motor2.getPulses();
nucho	0:3c49891bc39d	150
nucho	0:3c49891bc39d	151	encorder.data.length(2);
nucho	0:3c49891bc39d	152	encorder.data[0] = current1;
nucho	0:3c49891bc39d	153	encorder.data[1] = current2;
nucho	0:3c49891bc39d	154	encorderOut.write();
nucho	0:3c49891bc39d	155
nucho	0:3c49891bc39d	156	int ctrl1,ctrl2;
nucho	0:3c49891bc39d	157	ctrl1 = pid_motor1.compute(current1);
nucho	0:3c49891bc39d	158	ctrl2 = pid_motor2.compute(current2);
nucho	0:3c49891bc39d	159
nucho	0:3c49891bc39d	160	motor1.pulsewidth_us(ctrl1+MOTOR_OFFSET);
nucho	0:3c49891bc39d	161	motor2.pulsewidth_us(ctrl2+MOTOR_OFFSET);
nucho	0:3c49891bc39d	162
nucho	0:3c49891bc39d	163	return RTC_OK;
nucho	0:3c49891bc39d	164	}
nucho	0:3c49891bc39d	165
nucho	0:3c49891bc39d	166
nucho	0:3c49891bc39d	167	//////////////////////////////////////
nucho	0:3c49891bc39d	168	// on_error
nucho	0:3c49891bc39d	169	// This function is repeatedly called when
nucho	0:3c49891bc39d	170	// the RTno is in the ERROR condition.
nucho	0:3c49891bc39d	171	// The ERROR condition can be recovered,
nucho	0:3c49891bc39d	172	// when the RTno is reset.
nucho	0:3c49891bc39d	173	///////////////////////////////////////
nucho	0:3c49891bc39d	174	int RTno::onError() {
nucho	0:3c49891bc39d	175	return RTC_OK;
nucho	0:3c49891bc39d	176	}
nucho	0:3c49891bc39d	177
nucho	0:3c49891bc39d	178	////////////////////////////////////////
nucho	0:3c49891bc39d	179	// This function is called when
nucho	0:3c49891bc39d	180	// the RTno is reset. If on_reset is
nucho	0:3c49891bc39d	181	// succeeded, the RTno will enter into
nucho	0:3c49891bc39d	182	// the INACTIVE condition. If failed
nucho	0:3c49891bc39d	183	// (return value is RTC_ERROR), RTno
nucho	0:3c49891bc39d	184	// will stay in ERROR condition.ec
nucho	0:3c49891bc39d	185	///////////////////////////////////////
nucho	0:3c49891bc39d	186	int RTno::onReset() {
nucho	0:3c49891bc39d	187	return RTC_OK;
nucho	0:3c49891bc39d	188	}

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@0:3c49891bc39d, 2011-07-29 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning