Vector3 SphereBody::rungeVel(Vector3 x, real_t dt){
Vector3 K2, K1, K3, K4;
real_t t = 0;
K1 = (dt * getAcc(x,t));
K2 = (dt * getAcc((x+(1/2 * K1)), (t + 1/2*dt)));
K3 = (dt * getAcc((x+(1/2 * K2)), (t+1/2*dt)));
K4 = (dt * getAcc((x+K3), (t+dt)));
Vector3 runge = (x + ((K1 + 2 * K2 + 2 * K3 + K4)/6));
return runge;
}
void L6470::Dump(void) {
#ifndef NDEBUG
uint8_t reg;
printf("Registers:\n");
printf("01:ABS_POS - Current position: %ld\n", getPos());
printf("02:EL_POS - Electrical position: %ld\n", getParam(L6470_PARAM_EL_POS));
printf("03:MARK - Mark position: %ld\n", getMark());
printf("04:SPEED - Current speed: %ld (raw)\n", getParam(L6470_PARAM_SPEED));
printf("05:ACC - Acceleration: %.2f steps/s2\n", getAcc());
printf("06:DEC - Deceleration: %.2f steps/s2\n", getDec());
printf("07:MAX_SPEED - Maximum speed: %.2f steps/s\n", getMaxSpeed());
printf("08:MIN_SPEED - Minimum speed: %.2f steps/s\n", getMinSpeed());
printf("09:KVAL_HOLD - Holding KVAL: %d\n", (int) getHoldKVAL());
printf("0A:KVAL_RUN - Constant speed KVAL: %d\n", (int) getRunKVAL());
printf("0B:KVAL_ACC - Acceleration starting KVAL: %d\n", (int) getAccKVAL());
printf("0C:KVAL_Dec - Deceleration starting KVAL: %d\n", (int) getDecKVAL());
printf("0D:INT_SPEED - Intersect speed: 0x%.4X (raw)\n", (unsigned int) getParam(L6470_PARAM_INT_SPD));
reg = getParam(L6470_PARAM_ST_SLP);
printf("0E:ST_SLP - Start slope: %.3f\%% s/step (0x%.2X)\n", (float) 100 * SlpCalcValueReg(reg), reg);
reg = getParam(L6470_PARAM_FN_SLP_ACC);
printf("0F:FN_SLP_ACC - Acceleration final slope: %.3f\%% s/step (0x%.2X)\n", (float) 100 * SlpCalcValueReg(reg), reg);
reg = getParam(L6470_PARAM_FN_SLP_DEC);
printf("10:FN_SLP_DEC - Deceleration final slope: %.3f\%% s/step (0x%.2X)\n", (float) 100 * SlpCalcValueReg(reg), reg);
reg = getParam(L6470_PARAM_K_THERM);
printf("11:K_THERM - Thermal compensation factor: %.3f (0x%.2X)\n", KThermCalcValueReg(reg), reg);
reg = getParam(L6470_PARAM_ADC_OUT);
printf("12:ADC_OUT - ADC output: 0x%.2X\n", reg & 0x1F);
reg = getOCThreshold();
printf("13:OCD_TH - OCD threshold: %d mA (0x%.2X)\n", OcdThCalcValueReg(reg), reg);
reg = getParam(L6470_PARAM_STALL_TH);
printf("14:STALL_TH - STALL threshold: %d mA (0x%.2X)\n", StallThCalcValueReg(reg & 0x7F), reg);
printf("15:FS_SPD - Full-step speed: %.2f steps/s\n", getFullSpeed());
printf("16:STEP_MODE - Step mode: %d microsteps\n", 1 << getStepMode());
printf("17:ALARM_EN - Alarm enable: 0x%.2X\n", (unsigned int) getParam(L6470_PARAM_ALARM_EN));
printf("18:CONFIG - IC configuration: 0x%.4X\n", (unsigned int) getParam(L6470_PARAM_CONFIG));
#endif
}
void* keeperMotionThread(void* params)
{
void ** paramsList = (void**)params;
float &targetDist = *((float*) paramsList[0]);
float &moveDist = *((float*) paramsList[1]);
int &v_level= *((int*) paramsList[2]);
Robot &robot= *((Robot*) paramsList[3]);
bool &kmt_abort = *((bool*) paramsList[4]);
bool &rotating = *((bool*) paramsList[5]);
while(!robot.abort)
{
moveDist += v_level*DELTA_V*DELTA_T/float(1e6);
int acc = getAcc(v_level,targetDist-moveDist);
v_level += acc;
cv::Point2f tar_dir(robot.ownGoal_frontDir.y,-robot.ownGoal_frontDir.x);
if(v_level==0 && acc==0)
{
cv::Point2f ori_dir(sin(robot.ori),cos(robot.ori));
if(acos(ori_dir.dot(tar_dir))>ORI_TOL*M_PI/180)
{
rotating=true;
robot.rotateTo(tar_dir);
rotating=false;
}
}
sendAA(v_level*DELTA_V,v_level*DELTA_V);
//printf("dist = %f\n",targetDist-moveDist);
usleep(DELTA_T);
}
while(v_level)
{
v_level += v_level>0?-1:1;
sendAA(v_level*DELTA_V,v_level*DELTA_V);
//printf("dist = %f\n",targetDist-moveDist);
usleep(DELTA_T);
}
kmt_abort = true;
}
void ImuAdis16448::processMeasurements() {
while (1) {
boost::this_thread::interruption_point();
//get the newest measurement
Measurement::Ptr meas = Sensor::measurement_queue_.pop();
//VISENSOR_DEBUG("in imu_adis thread %d\n", meas.buffersize);
//check for missed frames
// TODO(schneith): use a timer to check for missed frames (currently we only detected sequences of MISSED-RECEIVED-MISSED, not MISSED-MISSED,...)
if( checkTimestamp(meas->timestamp) )
{
//publish an empty missed frame
ViImuMsg::Ptr missed_imu_ptr = boost::make_shared<ViImuMsg>();
missed_imu_ptr->imu_id = Imu::imu_id_;
publishImuData(missed_imu_ptr, ViErrorCodes::MEASUREMENT_DROPPED);
}
// //TODO adapt
// create new shared pointer for the imu message
ViImuMsg::Ptr imu_msg_ptr = boost::make_shared<ViImuMsg>();
// Add imu information to the msg
imu_msg_ptr->imu_id = Imu::imu_id_;
imu_msg_ptr->timestamp=meas->timestamp;
imu_msg_ptr->timestamp_host = meas->timestamp_host;
imu_msg_ptr->timestamp_fpga_counter = meas->timestamp_fpga_counter;
// get imu data
getGyro(meas->data.get(), &imu_msg_ptr->gyro[0]);
getAcc(meas->data.get(), &imu_msg_ptr->acc[0]);
getMag(meas->data.get(), &imu_msg_ptr->mag[0]);
getBaro(meas->data.get(), &imu_msg_ptr->baro);
publishImuData(imu_msg_ptr, ViErrorCodes::NO_ERROR);
}
}
void AMU::receiveDataCallback(const can_msgs::CANFrame::ConstPtr& msg)
{
// Restore angle
double angle[3] = {0.0, 0.0, 0.0}; // [deg]
if(msg->id == 0x41)
{
for(int i = 0; i < 3; i++)
{
angle[i] = getAngle(msg->data[i*2], msg->data[i*2+1]);
}
ROS_INFO("Roll: %3.1f, Pitch: %3.1f, Yaw: %3.1f [deg]", angle[0], angle[1], angle[2]);
}
// Restore angular velocity
double angular_vel[3] = {0.0, 0.0, 0.0}; // [deg/sec]
if(msg->id == 0x42)
{
for(int i = 0; i < 3; i++)
{
angular_vel[i] = getAngularVel(msg->data[i*2], msg->data[i*2+1]);
}
ROS_INFO("Roll vel: %3.1f, Pitch vel: %3.1f, Yaw vel: %3.1f [deg/sec]", angular_vel[0], angular_vel[1], angular_vel[2]);
}
// Restore translational acceleration
double acc[3] = {0.0, 0.0, 0.0}; // [G]
if(msg->id == 0x43)
{
for(int i = 0; i < 3; i++)
{
acc[i] = getAcc(msg->data[i*2], msg->data[i*2+1]);
}
ROS_INFO("Acc_x: %3.1f, Acc_y: %3.1f, Acc_z: %3.1f [m/sec^2]", acc[0], acc[1], acc[2]);
}
// Publish Topic
sensor_msgs::Imu data;
// Angle [rad] -> Quaternion
// Remark: Coordinate between KAMUI and AMU is different. (y- and z-axis are reverse)
// ** KAMUI **
// x+: forward, y+: left, z+: above
// ** AMU **
// x+: forward, y+: right, z+: below
// You must reverse the sign of pitch and yaw, and also corresponding angular velocities.
double roll = angle[0] * DEG_TO_RAD;
double pitch = -angle[1] * DEG_TO_RAD;
double yaw = -angle[2] * DEG_TO_RAD;
data.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
// Angular velocity [rad/s]
data.angular_velocity.x = angular_vel[0] * DEG_TO_RAD;
data.angular_velocity.y = -angular_vel[1] * DEG_TO_RAD;
data.angular_velocity.z = -angular_vel[2] * DEG_TO_RAD;
// Linear acceleration [m/sec^2]
data.linear_acceleration.x = acc[0] * G_TO_MS2;
data.linear_acceleration.y = -acc[1] * G_TO_MS2;
data.linear_acceleration.z = -acc[2] * G_TO_MS2;
// Covariance
// TODO: Check the definition of covariance / 20150807, Katsumoto
data.orientation_covariance[0] = -1;
data.angular_velocity_covariance[0] = -1;
data.linear_acceleration_covariance[0] = -1;
// Publish
imu_pub_.publish(data);
}
auto V30MZ::opMoveMemAcc(Size size) {
write(size, segment(r.ds), fetch(Word), getAcc(size));
}
void Projectile::Move()
{
// Reset the acceleration each time in case we don't need to move.
setAcc(Vector2(0, 0));
if (attacking)
{
if (timerSet == false)
{
currentAttackTimer = attackTimer + sfw::getTime();
timerSet = true;
}
// Check what direction brings us towards the destination.
if (dest.x > getPos().x - 20)
{
if(getVel().x < -0.5f)
setAcc(Vector2(attackSpeed * 4, getAcc().y));
else
setAcc(Vector2(attackSpeed, getAcc().y));
}
else if (dest.x < getPos().x + 20)
{
if(getVel().x > 0.5f)
setAcc(Vector2(-attackSpeed * 4, getAcc().y));
else
setAcc(Vector2(-attackSpeed, getAcc().y));
}
if (dest.y > getPos().y - 20)
{
if(getVel().y < -0.5f)
setAcc(Vector2(getAcc().x, attackSpeed * 4));
else
setAcc(Vector2(getAcc().x, attackSpeed));
}
else if (dest.y < getPos().y + 20)
{
if(getVel().y > 0.5f)
setAcc(Vector2(getAcc().x, -attackSpeed * 4));
else
setAcc(Vector2(getAcc().x, -attackSpeed));
}
if (attackTimer < sfw::getTime())
{
attacking = false;
timerSet = false;
}
else if(getPos().x > dest.x - 5 && getPos().x < dest.x + 5 &&
getPos().y > dest.y - 5 && getPos().y < dest.y + 5)
{
attacking = false;
timerSet = false;
}
}
else
{
// Check what direction brings us towards the player.
if (game()->getPlayer().getPos().x >= getPos().x)
{
if (getVel().x < -0.5f)
setAcc(Vector2(accRate * 1.5f, getAcc().y));
else
setAcc(Vector2(accRate, getAcc().y));
}
else if (game()->getPlayer().getPos().x < getPos().x)
{
if (getVel().x > 0.5f)
setAcc(Vector2(-accRate * 1.5f, getAcc().y));
else
setAcc(Vector2(-accRate, getAcc().y));
}
if (game()->getPlayer().getPos().y >= getPos().y)
{
if (getVel().y < -0.5f)
setAcc(Vector2(getAcc().x, accRate * 1.5f));
else
setAcc(Vector2(getAcc().x, accRate));
}
else if (game()->getPlayer().getPos().y < getPos().y)
{
if (getVel().y > 0.5f)
setAcc(Vector2(getAcc().x, -accRate * 1.5f));
else
setAcc(Vector2(getAcc().x, -accRate));
}
}
// Apply the acceleration to the velocity
if (getAcc().x == 0 && getAcc().y == 0)
setVel(Vector2(getVel().x / 4, getVel().y / 4));
else if (getAcc().x == 0)
setVel(Vector2(getVel().x / 4, getVel().y + (getAcc().y * sfw::getDeltaTime())));
else if (getAcc().y == 0)
setVel(Vector2(getVel().x + (getAcc().x * sfw::getDeltaTime()), getVel().y / 4));
else
setVel(Vector2(getVel().x + (getAcc().x * sfw::getDeltaTime()), getVel().y + (getAcc().y * sfw::getDeltaTime())));
applyMaxSpeed();
// Use all the information we just calculated to move accordingly
setPos(Vector2(getPos().x + getVel().x, getPos().y + getVel().y));
}
auto V30MZ::opOutDX(Size size) {
wait(5);
out(size, r.dx, getAcc(size));
}
auto V30MZ::opOut(Size size) {
wait(5);
out(size, fetch(), getAcc(size));
}
int main(void){
atexit(destory);
signal(SIGINT,sig_exit);
init();
acc = malloc(sizeof(Acc));
Msg msg1,msg2;
int choose;
begin();
while(1){
int mark_var = 0;
int login_var = 0;
scanf("%d",&choose);
switch(choose){
case 0:
exit(0);
case 1:
//向服务器发送消息
msg1.mtype = C_CRTACC;
//写一个函数用于获取用户输入的密码和帐号
getAcc(acc);
//将密码和帐号放入acc中,再将acc放入消息队列中
msg1.acc = *acc;
if(-1 == msgsnd(msgid1,&msg1,sizeof(msg1),0)){
perror("发送消息失败");
exit(-1);
}
if(-1 == msgrcv(msgid2,&msg2,sizeof(msg2),0,0)){
perror("消息接收失败");
exit(-1);
}
printf("正在提交用户数据,请稍后...\n");
sleep(1);
if(S_SUCCESS == msg2.mtype){
printf("开户成功\n");
printf("您的账户ID为%d\n用户名为%s\n余额为%.2f\n",
msg2.acc.id,msg2.acc.name,msg2.acc.money);
}
else if(S_CRTRENAME == msg2.mtype){
printf("该用户名已被注册，请重新操作\n");
sleep(2);
begin();
continue;
}
else if(S_FAILED == msg2.mtype){
printf("开户失败,请重新操作\n");
sleep(2);
begin();
continue;
}
mark_var = 1;
break;
case 2:
printf("请输入用户名:");
scanf("%s",acc->name);
init_io(1);
printf("请输入用户密码:");
scanf("%s",acc->pass);
init_io(0);
acc->id = 0;
acc->money = 0;
msg1.mtype = C_LOGIN;
msg1.acc = *acc;
if(-1 == msgsnd(msgid1,&msg1,sizeof(msg1),0)){
perror("发送消息失败");
exit(-1);
}
printf("\n登录中...\n");
sleep(1);
if(-1 == msgrcv(msgid2,&msg2,sizeof(msg2),0,0)){
perror("登录失败");
exit(-1);
}
if(S_SUCCESS == msg2.mtype){
printf("登录成功\n");
login_var = 1;
}
else if(S_FAILED == msg2.mtype){
login_var = 0;
printf("登录失败,请检查用户名密码是否正确\n");
mark_var = 1;
}
break;
case 3:
printf("请输入用户名:");
scanf("%s",acc->name);
init_io(1);
printf("请输入密码:");
scanf("%s",acc->pass);
init_io(0);
msg1.mtype = C_DELACC;
msg1.acc = *acc;
if(-1 == msgsnd(msgid1,&msg1,sizeof(msg1),0)){
perror("\n发送消息失败");
exit(-1);
}
printf("\n正在销户,请稍后...\n");
sleep(1);
if(-1 == msgrcv(msgid2,&msg2,sizeof(msg2),0,0)){
perror("\n销户失败");
continue;
}
if(S_SUCCESS == msg2.mtype){
printf("销户成功\n");
}
else if(S_FAILED == msg2.mtype){
printf("销户失败\n");
}
else if(S_PSWRONG == msg2.mtype){
printf("密码错误\n");
}
mark_var = 1;
break;
default:
printf("您的输入有误,请重新输入!!\n");
break;
}
if(mark_var){
scanf("%*c");
printf("按回车键返回上一层\n");
scanf("%*c");
begin();
}
if(login_var == 1)
break;
}
*acc = msg2.acc;
acc->balance = 0;
strcpy(acc->other,"");
OldAcc(acc->name);
while(1){
scanf("%d",&choose);
switch(choose){
case 0:
exit(0);
case 1:
printf("请输入存款金额:");
scanf("%lg",&(acc->balance));
msg1.mtype = C_DEPOSIT;
msg1.acc = *acc;
if(-1 == msgsnd(msgid1,&msg1,sizeof(msg1),0)){
perror("发送消息失败");
exit(-1);
}
printf("正在操作,请稍后...\n");
sleep(1);
if(-1 == msgrcv(msgid2,&msg2,sizeof(msg2),0,0)){
perror("存款失败");
continue;
}
if(S_SUCCESS == msg2.mtype){
printf("存款成功\n");
}
else if(S_FAILED == msg2.mtype){
printf("存款失败\n");
}
break;
case 2:
label:printf("请输入取款金额:");
scanf("%lg",&(acc->balance));
if(acc->money < acc->balance){
printf("您的余额不足,请重新输入\n");
goto label;
}
msg1.mtype = C_DRAWMONEY;
msg1.acc = *acc;
if(-1 == msgsnd(msgid1,&msg1,sizeof(msg1),0)){
perror("发送消息失败");
exit(-1);
}
printf("正在操作,请稍后...\n");
sleep(1);
if(-1 == msgrcv(msgid2,&msg2,sizeof(msg2),0,0)){
perror("取款失败");
continue;
}
if(S_SUCCESS == msg2.mtype){
printf("取款成功\n");
}
else if(S_FAILED == msg2.mtype){
printf("取款失败\n");
}
break;
case 3:
printf("请输入对方的账户:");
scanf("%s",acc->other);
label1:printf("请输入要转账的金额:");
scanf("%lg",&(acc->balance));
if(acc->money < acc->balance){
printf("您的余额不足,请重新输入\n");
goto label1;
}
msg1.mtype = C_TRANSFER;
msg1.acc = *acc;
if(-1 == msgsnd(msgid1,&msg1,sizeof(msg1),0)){
perror("发送消息失败");
exit(-1);
}
printf("正在操作,请稍后...\n");
sleep(1);
if(-1 == msgrcv(msgid2,&msg2,sizeof(msg2),0,0)){
perror("转账失败");
continue;
}
if(S_SUCCESS == msg2.mtype){
printf("转账成功\n");
}
else if(S_FAILED == msg2.mtype){
printf("转账失败\n");
}
else if(S_NOSUCHUSER == msg2.mtype){
printf("不存在该用户,请重新输入\n");
}
break;
case 4:
msg1.mtype = C_CHECKMONEY;
msg1.acc = *acc;
if(-1 == msgsnd(msgid1,&msg1,sizeof(msg1),0)){
perror("发送消息失败");
exit(-1);
}
printf("正在查询,请稍后...\n");
sleep(1);
if(-1 == msgrcv(msgid2,&msg2,sizeof(msg2),0,0)){
perror("查询失败");
continue;
}
if(S_SUCCESS == msg2.mtype){
printf(" 用户名:\t%s\n",msg2.acc.name);
printf(" 账户id:\t%d\n",msg2.acc.id);
printf("账户余额:\t%.2f\n",msg2.acc.money);
}
else if(S_FAILED == msg2.mtype){
printf("查询失败\n");
}
break;
default:
printf("您的输入有误,请重新输入\n");
break;
}
scanf("%*c");
printf("按回车键返回上一层\n");
scanf("%*c");
OldAcc(acc->name);
}
return 0;
}
