robotlabmate.h

基于vc 的环境下机器人自主避障的算法 图形处理 具有载物功能

//////////////////////////////////////////////////////////////////////
// MuRoS - Multi Robot Simulator
//
// Luiz Chaimowicz
// GRASP Lab. University of Pennsylvania
// VERLab - DCC - UFMG - Brasil
//
// RobotLabmate.h: interface for the CRobotLabmate class.
//
//////////////////////////////////////////////////////////////////////

#if !defined(ROBOTLABMATE_H)
#define ROBOTLABMATE_H

#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000

#include "RobotNonHolonomic.h"
#include "RobotXR4000.h"
#include "MapPath.h"
#include "Polygon.h"


class CRobotLabmate : public CRobotNonHolonomic  
{
public:
	DECLARE_SERIAL(CRobotLabmate)


	CRobotLabmate();
	CRobotLabmate(double x, double y, double theta, short status, short id, CString name);
	virtual ~CRobotLabmate();

	virtual void Serialize(CArchive& archive );
	virtual void Draw(CDC* pDC);
	virtual void Output(double Simtime);
	virtual void Update(CArray<CRobot*, CRobot*> *robots, double simTime, double dt, CBox* box, CMapPath *mapPath);
	virtual void DetectBox(CBox *box);
	virtual void SetOrientation(double angle);
	virtual void ForceOnBoxes(CBox *box,double &fx, double &fy, double &ftheta);

	virtual afx_msg LRESULT OnControlMsg(WPARAM wParam, LPARAM lParam);
	virtual afx_msg LRESULT OnDataMsg(WPARAM wParam, LPARAM lParam);


private:
	void ComputeForces(CBox* box);
	void ComputeArmPos();
	void InitializeArmBox(CPoint, CBox* box, double theta);
	void ChangeLeader(short leader, double posX, double posY);
	void BackupManeuver(double simTime, double dt);

	double m_sizex;		// x dimension of the labmate
	double m_sizey;		// y dimension of the labmate
	
	double m_armB[3];	// arm position relative to the box
	double m_armW[3];	// arm position relative to the world
	double m_armR[3];	// arm position relative to the robot
	double m_armA[3];	// arm position relative to the arm frame

	double m_tA2R[3];	// conversion from the arm frame to the robot frame (x,y,theta)
	double m_tR2A[3];	// conversion from the robot frame to the arm frame (x,y,theta)

	double m_force[3];		// forces aplied in the object (robot coordinates)
	double m_force_w[3];	// forces aplied in the object (world coordinates)
	

	CControlMsg* m_controlMsgPoll[20];	// list of received control messages (***** change later)

	double m_backupAngle;				// variables for the backu maneuver
	double m_backupAngleFinal;
	double m_beginLead;

	CPolygon m_polygon;					// polygon used to draw the robot
};

#endif