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; }