#include<iostream>
#include <Eigen/Dense>
#include <math.h>
#include <vector>
using namespace std;
using namespace Eigen;
using Eigen::MatrixXd;
const double Dk=3.1415926/; //定义Frame(二维笛卡尔坐标系)类
class Frame{
private:
double Ox;
double Oy;//Frame对象在WF(世界坐标系)下的原点坐标
double Angle;//方位角(弧度制)
public:
Frame();
Frame(double ox,double oy,double an);
MatrixXd CalPose();//获得齐次位姿矩阵
};
Frame::Frame() {
Ox=;Oy=;Angle=;
}
Frame::Frame(double ox,double oy,double an){
Ox=ox;Oy=oy;Angle=an*Dk;//输入为角度制
}
MatrixXd Frame::CalPose(){
MatrixXd Pose(,);
Pose(,)=cos(Angle);Pose(,)=-sin(Angle);Pose(,)=Ox;
Pose(,)=sin(Angle);Pose(,)=cos(Angle);Pose(,)=Oy;
Pose(,)=;Pose(,)=;Pose(,)=;
return Pose;
}//由原点和方位角计算位姿矩阵
//Frame定义结束 //定义Solver(运动学求解器)类
class Solver{
private:
vector<Frame> frameVec;
public:
MatrixXd CalTheta(double l1,double l2,Frame frame,double px,double py);//运动学反解
void addFrame(Frame frame);//添加任务框
void deleteFrame(); //移除任务框
};
MatrixXd Solver::CalTheta(double l1,double l2,Frame frame,double px,double py) {
MatrixXd P_task(,),P_world(,),T(,);
P_task(,)=px;P_task(,)=py;P_task(,)=;
P_world=frame.CalPose()*P_task;
T(,)=acos((px*px+py*py-l1*l1-l2*l2)//l1/l2);//取0°到180°
double beta=atan2(py,px);
double lp=sqrt(px*px+py*py);//端点模值
double fea=acos((lp*lp+l1*l1-l2*l2)//l1/lp);
T(,)=beta-fea;
return T;
}
void Solver::addFrame(Frame frame) {
frameVec.push_back(frame);
}
void Solver::deleteFrame() {
frameVec.pop_back();
}
//Solver类定义结束 //定义Robot类
class Robot{
public:
double L1;
double L2;//臂长
double theta1;
double theta2;//SCARA关节变量 (弧度角) Robot();
Robot(double l1,double l2,double t1,double t2);
Vector2d getTerminal();//手臂末端的世界坐标(笛卡尔坐标) ,正运动学计算
void ShowState();//状态显示
void PTPmove(Frame frame,double px,double py);//点到点运动 };
Robot::Robot() {
L1=;L2=;theta1=;theta2=;
};
Robot::Robot(double l1,double l2,double t1,double t2){
L1=l1;L2=l2;theta1=t1*Dk;theta2=t2*Dk;//输入为角度制角
}
Vector2d Robot::getTerminal() {
double Tx,Ty;
Tx=L1*cos(theta1)+L2*cos(theta1+theta2);
Ty=L1*sin(theta1)+L2*sin(theta1+theta2);
Vector2d T(Tx,Ty);
return T;
}
void Robot::ShowState(){
cout<<"theta1="<<theta1/Dk<<endl<<"theta2="<<theta2/Dk<<endl;
}
void Robot::PTPmove(Frame frame,double px,double py){
Solver solver;
MatrixXd T(,);
T=solver. CalTheta(L1,L2,frame,px,py);
theta1=T(,);theta2=T(,);
}
//Robot定义结束 int main(){
Robot myRobot(,,,);//构造Robot对象
Frame TF1(,,),TF2(-,,),TF3(0.5,1.5,);//定义任务坐标系
myRobot.ShowState();
myRobot.PTPmove(TF1,,);
myRobot.ShowState();
myRobot.PTPmove(TF2,,);
myRobot.ShowState();
return ;
}

实时控制软件设计 第二次作业 myRobot的更多相关文章

  1. 实时控制软件设计第一周作业-汽车ABS软件系统案例分析

    汽车ABS软件系统案例分析 ABS 通过控制作用于车轮制动分泵上的制动管路压力,使汽车在紧急刹车时车轮不会抱死,这样就能使汽车在紧急制动时仍能保持较好的方向稳定性. ABS系统一般是在普通制动系统基础 ...

  2. 《实时控制软件设计》之Github提交作业步骤

    在掌握GIT/GITHUB基本操作后,接下来把第一次的编程作业提交到 https://github.com/RTCSD15/HOMEWORK1 ,把第二次的编程作业提交到https://github. ...

  3. 实时控制软件设计作业_01——汽车ABS系统分析

    制动防抱死系统(antilock brake system)简称ABS.作用就是在汽车制动时,自动控制制动器制动力的大小,使车轮不被抱死,处于边滚边滑(滑移率在20%左右)的状态,以保证车轮与地面的附 ...

  4. 《实时控制软件设计》第一周作业 欧梓峰 U201317662 (更新)

    CNC 插补计算程序分析 前言:插补(Interpolation),即机床数控系统依照一定方法确定刀具运动轨迹的过程.一般是已知起点坐标.终点坐标和轨迹,由数控插补计算程序实时的算出各个中间的坐标来拟 ...

  5. 实时控制软件设计 第一次作业 Draw

    #include <iostream> #include <cstring> #include <math.h> #include <Eigen/Dense& ...

  6. 《实时控制软件设计》之Automation Studio开发环境

    Automation Studio是贝加莱公司的控制软件开发平台,软件可运行在贝加莱的基于PC的控制器上,基于Automation Studio我们可构建一个完整的控制软件构建.测试和仿真运行平台.本 ...

  7. 《实时控制软件设计》Git 基本操作练习

    根据老师提供的教程 对 数据库创建.提交文件.创建分支.删除分支.合并分支.冲突处理等操作进行了练习 得到log文件如下: yanbin-guo@yanbinguo MINGW64 /Git (mas ...

  8. OO--第三单元规格化设计 博客作业

    OO--第三单元规格化设计 博客作业 前言 第三单元,我们以JML为基础,先后完成了 PathContainer -> Graph -> RailwaySystem 这是一个递进的过程,代 ...

  9. 2017-2018-1 20179205《Linux内核原理与设计》第二周作业

    <Linux内核原理与分析>第二周作业 本周视频学习情况: 通过孟老师的视频教程,大致对风诺依曼体系结构有了一个初步的认识,视频从硬件角度和程序员角度对CPU和Main Memory(内存 ...

随机推荐

  1. MVC5+EF6 入门完整教程七

    本篇我们针对表格显示添加一些新功能. 前面我们已经讲解过表格显示数据了,现在我们添加三个常用功能: 对显示结果进行排序.过滤.分页. 文章提纲 理论基础/前置准备 详细步骤 总结 前置准备 – 应用之 ...

  2. C# Out,Ref 学习总结

    C# Out,Ref 学习总结. ref是传递参数的地址,out是返回值,两者有一定的相同之处,不过也有不同点. 使用ref前必须对变量赋值,out不用. out的函数会清空变量,即使变量已经赋值也不 ...

  3. zookeeper学习系列:四、Paxos算法和zookeeper的关系

    一.问题起源 淘宝搜索的博客 http://www.searchtb.com/2011/01/zookeeper-research.html  提到Paxos是zookeeper的灵魂 有一篇文章标题 ...

  4. jquery触屏幻灯片

    一.前言 去年接触了移动Web开发,做了些手机端的网站及应用,还有些小的微信游戏和活动页面.每个项目里或多或少的都会有一些触屏事件等.其中有两个用到了jquery触屏幻灯片.刚开始的时候也在百度上搜索 ...

  5. 前端面试题 之 JavaScript

    昨天我们一起分享了关于html和css的面试题<前端面试题之Html和CSS>,今天我们来分享关于javascript有关的面试题.我面试的时候最害怕面试官问我js了,因为我真心不擅长这个 ...

  6. 用HTML做的简单的个人简历

    <html> <head> <title>table表格</title> <style type="text/css"> ...

  7. μC/OS-Ⅲ系统中的任务就续表

    μC/OS-Ⅲ支持任意数目的不同优先级.一般情况下64级优先级就足够了. 在μC/OS-Ⅲ中所有已经就绪等待运 行的任务都被放入一个我为的“就续表”(ready list)中.就续表包括两部分:一个就 ...

  8. Ubuntu下Nutch1.2的使用

    aaarticlea/png;base64,iVBORw0KGgoAAAANSUhEUgAAAeMAAABpCAIAAACGSdxlAAAAA3NCSVQICAjb4U/gAAAgAElEQVR4Xu ...

  9. cygwin下载指南(转)

    http://blog.chinaunix.net/uid-20178959-id-1731456.html

  10. SIGKDD历年Best Papers

    作者:我爱机器学习原文链接:SIGKDD历年Best Papers SIGKDD(Data Mining)(1997-2016) 年份 标题 一作 一作单位 2016 FRAUDAR: Boundin ...