/*! Calculates the current velocity during scrolling
*/
QPointF QKineticScrollerPrivate::calculateVelocity(qreal time)
{
QPointF velocity;
// -- x coordinate
if (overshootX) {
if (overshootSpringConstantRoot * (time-overshootStartTimeX) < M_PI) // prevent swinging around
velocity.setX(overshootVelocity.x() * qCos(overshootSpringConstantRoot * (time - overshootStartTimeX)));
else
velocity.setX(-overshootVelocity.x());
} else {
qreal newVelocity = decelerate(releaseVelocity.x(), time);
if (scrollToX) {
if (qAbs(newVelocity) < qreal(30.0 / 1000) /* 30mm/s */)
newVelocity = qreal(30.0 / 1000) * qSign(releaseVelocity.x());
} else {
if (qAbs(newVelocity) < qreal(0.5) * qreal(framesPerSecond) / pixelPerMeter /* 0.5 [pix/frame] */)
newVelocity = 0;
}
velocity.setX(newVelocity);
}
// -- y coordinate
if (overshootY) {
if (overshootSpringConstantRoot * (time-overshootStartTimeY) < M_PI) // prevent swinging around
velocity.setY(overshootVelocity.y() * qCos(overshootSpringConstantRoot * (time - overshootStartTimeY)));
else
velocity.setY(-overshootVelocity.y());
} else {
qreal newVelocity = decelerate(releaseVelocity.y(), time);
if (scrollToY) {
if (qAbs(newVelocity) < qreal(30.0 / 1000) /* 30mm/s */)
newVelocity = qreal(30.0 / 1000) * qSign(releaseVelocity.y());
} else {
if (qAbs(newVelocity) < qreal(0.5) * qreal(framesPerSecond) / pixelPerMeter /* 0.5 [pix/frame] */)
newVelocity = 0;
}
velocity.setY(newVelocity);
}
return velocity;
}
void WaypointLoaderNode::loadWaypointsForVer1(const char *filename, std::vector<autoware_msgs::waypoint> *wps)
{
std::ifstream ifs(filename);
if (!ifs)
return;
std::string line;
std::getline(ifs, line); // Remove first line
while (std::getline(ifs, line))
{
autoware_msgs::waypoint wp;
parseWaypointForVer1(line, &wp);
wps->push_back(wp);
}
size_t last = wps->size() - 1;
for (size_t i = 0; i < wps->size(); ++i)
{
if (i != last)
{
double yaw = atan2(wps->at(i + 1).pose.pose.position.y - wps->at(i).pose.pose.position.y,
wps->at(i + 1).pose.pose.position.x - wps->at(i).pose.pose.position.x);
wps->at(i).pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
}
else
{
wps->at(i).pose.pose.orientation = wps->at(i - 1).pose.pose.orientation;
}
wps->at(i).twist.twist.linear.x = decelerate(
wps->at(i).pose.pose.position, wps->at(wps->size() - 1).pose.pose.position, wps->at(i).twist.twist.linear.x);
}
}
int umain(){
robot_id = 1;
copy_objects();
copy_objects();
field_state.curr_loc.x = (int)(game.coords[0].x * VPS_RATIO);
field_state.curr_loc.y = (int)(game.coords[0].y * VPS_RATIO);
field_state.curr_angle = (((float)game.coords[0].theta) * 180.0) / 2048;
gyro_set_degrees(field_state.curr_angle);
initialize();
/*for(int sextant = 0; sextant < 6; sextant++){
printf("TPX: %d, TPY: %d, LPX: %d, LPY: %d\n", get_territory_pivot_point_loc(sextant).x, get_territory_pivot_point_loc(sextant).y, lever_pivot_points[2*sextant], lever_pivot_points[2*sextant + 1]);
}*/
init_pid(&field_state.circle_PID, KP_CIRCLE, KI_CIRCLE, KD_CIRCLE, &get_angle_error_circle, &update_circle_velocities);
field_state.circle_PID.enabled = true;
field_state.circle_PID.goal = 0;
//printf("OX: %d, OY: %d, TX: %d, TY: %d, TWX: %d, TWY: %d, CA: %.2f, G: %.3f, LE: %d, RE: %d\n", field_state.curr_loc.x, field_state.curr_loc.y, field_state.target_loc.x, field_state.target_loc.y, field_state.target_loc_waypoint.x, field_state.target_loc_waypoint.y, field_state.curr_angle, gyro_get_degrees(), encoder_read(LEFT_ENCODER_PORT), encoder_read(RIGHT_ENCODER_PORT));
while(1){
update_field();
accelerate();
decelerate();
if(field_state.pid_enabled)
update_pid(&field_state.circle_PID);
if(get_time() > 119000){
}
//printf("Sextant: %d, OX: %d, OY: %d, TX: %d, TY: %d, TWX: %d, TWY: %d, CA: %.2f, G: %.3f, LE: %d, RE: %d\n", get_current_sextant(field_state.curr_loc), field_state.curr_loc.x, field_state.curr_loc.y, field_state.target_loc.x, field_state.target_loc.y, field_state.target_loc_waypoint.x, field_state.target_loc_waypoint.y, field_state.curr_angle, gyro_get_degrees(), encoder_read(LEFT_ENCODER_PORT), encoder_read(RIGHT_ENCODER_PORT));
}
return 0;
}
task main()
{
waitForStart();
initializeRobot();
zeroEncoders();
accelerate(100, 2000, 0, 50);
zeroEncoders();
decelerate(0, 250, 100, 10);
wait10Msec(100);
}
void player::idle()
{
if (speed <= 0)
{
speed = 0;
}
else if (speed > 0)
{
decelerate();
}
}
bool Car::activateBoost(){
if(boost > MAX_BOOST) boost = MAX_BOOST;
if(boost > 50){
boost -= 8;
speed = speed + std::max(2., 10*std::log(speed/10.));
speed = std::min(1.1f * MAX_SPEED, speed);
return true;
}
else{
decelerate();
return false;
}
}
/**
@description: function used for input keyboard
@input: shared structure
*/
void input_keyboard(struct car_t *c)
{
if(key[KEY_S])
ignition_on(c);
if(key[KEY_O])
ignition_off(c);
if(key[KEY_UP])
accelerate(c);
if(key[KEY_DOWN])
decelerate(c);
if(key[KEY_B])
break_pedal(c);
if(key[KEY_SPACE])
reduce_brake(c);
if(key[KEY_P])
shift_park(c);
if(key[KEY_R])
shift_reverse(c);
if(key[KEY_N])
shift_neutral(c);
if(key[KEY_D])
shift_drive(c);
if(key[KEY_C])
cruise_control_on(c);
if(key[KEY_X])
cruise_control_off(c);
if(key[KEY_W])
change_wheels(c);
if(key[KEY_F])
refill_fuel(c);
if(key[KEY_Q])
end_simulation = 1;
}
int main (void) {
TCNT0 = 0x63;
TCCR0 = 0x03;
DDRB = 0x1E;
PORTB = 0x1E;
int j=0;
while(1) {
accelerate();
wait(300);
decelerate();
wait(300);
}
return 0;
}
void WaypointLoaderNode::planningVelocity(std::vector<autoware_msgs::waypoint> *wps)
{
for (size_t i = 0; i < wps->size(); ++i)
{
wps->at(i).twist.twist.linear.x = decelerate(
wps->at(i).pose.pose.position, wps->at(wps->size() - 1).pose.pose.position, wps->at(i).twist.twist.linear.x);
}
if(!disableVelocitySmoothing_){
std::vector<autoware_msgs::waypoint> temp = *wps;
if(temp.size() > 3){
for (size_t i = 1; i< wps->size()-1; ++i){
wps->at(i).twist.twist.linear.x =
(temp.at(i-1).twist.twist.linear.x +
temp.at(i-1).twist.twist.linear.x +
temp.at(i-1).twist.twist.linear.x) / 3;
}
}
}
}
void Ship::correctSpeed()
{
float stopping_distance = sqrt(pow(getStoppingDistance(velocity.x, frame.getAccel()), 2.0f) + pow(getStoppingDistance(velocity.x, frame.getAccel()), 2.0f));
auto moving_toward = movingToward(target);
if (stopping_distance < getDistanceBetween(getPosition(), target) - getGlobalBounds().width / 2.0f &&
moving_toward.first && moving_toward.second)
{
accelerate();
}
else if (!moving_toward.first)
{
if (target.x > getPosition().x)
{
velocity.x += frame.getAccel() / 2.0f;
}
else
{
velocity.x -= frame.getAccel() / 2.0f;
}
}
else if (!moving_toward.second)
{
if (target.y > getPosition().y)
{
velocity.y += frame.getAccel() / 2.0f;
}
else
{
velocity.y -= frame.getAccel() / 2.0f;
}
}
else
{
decelerate();
}
}
waypoint_follower::lane createLaneWaypoint(std::vector<WP> waypoints)
{
waypoint_follower::lane lane_waypoint;
lane_waypoint.header.frame_id = "/map";
lane_waypoint.header.stamp = ros::Time(0);
for (unsigned int i = 0; i < waypoints.size(); i++)
{
waypoint_follower::waypoint wp;
wp.pose.header = lane_waypoint.header;
wp.pose.pose.position = waypoints[i].pose.position;
wp.pose.pose.orientation = waypoints[i].pose.orientation;
wp.twist.header = lane_waypoint.header;
double vel_kmh = decelerate(point2vector(waypoints[i].pose.position),
point2vector(waypoints[waypoints.size() -1 ].pose.position),
waypoints[i].velocity_kmh);
wp.twist.twist.linear.x = kmph2mps(vel_kmh);
lane_waypoint.waypoints.push_back(wp);
}
return lane_waypoint;
}
// Entry point, containing initialization and the main draw / update loop.
int main(void)
{
int i;
// Array of all existing stars.
Star stars[STAR_COUNT];
// Records whether warp / warp effect is active.
bool warping = FALSE;
// Records whether warp is currently accelerating or decelerating.
bool accelerating = TRUE;
// The number of frames the camera has been warping at full speed for.
int warpFrames = 0;
// The current speed of the camera.
float speed = MIN_SPEED;
// Lateral camera speed.
float strafeSpeed = 0.0f;
// An interpolated version of the camera speed for the HUD bars.
float smoothSpeed = MIN_SPEED;
// Seed the RNG with a carefully constructed non-arbitrary number.
srand(0x3ae14c92);
// Prepare the LCD display and motor for use.
lcd_init();
motor_init();
// Make sure the motor is going at the initial speed.
updateMotor(speed);
// Give each star a random starting position.
for (i = 0; i < STAR_COUNT; ++i) {
randomizeStar(&stars[i]);
stars[i].z = randFloat();
}
// Main loop.
for (;;) {
// Erase all the stars from the sky. I'm assuming it's faster to do
// this than do lcd_fillScreen(BLACK).
for (i = 0; i < STAR_COUNT; ++i) {
renderStar(stars[i], speed, BLACK);
}
strafeSpeed *= 0.95f;
// Strafe left when left button is pressed.
if (input_isKeyDown(BUTTON_LEFT)) {
strafeSpeed -= 0.001f;
}
// Likewise, but for strafing right.
if (input_isKeyDown(BUTTON_RIGHT)) {
strafeSpeed += 0.001f;
}
// If the centre key has been pressed, toggle warping.
if (input_getButtonPress() == BUTTON_CENTER) {
warping = !warping;
}
// If we aren't currently warping, accept button press inputs.
if (!warping) {
// Accelerate when pressing up.
if (input_isKeyDown(BUTTON_UP)) {
accelerate(&speed, MANUAL_ACCEL);
}
// Decelerate when pressing down.
if (input_isKeyDown(BUTTON_DOWN)) {
decelerate(&speed, MANUAL_ACCEL);
}
// Otherwise, if we are in the acceleration phase of warping, speed up
// the camera until it is at maximum speed.
} else if (accelerating && accelerate(&speed, BOOST_ACCEL)) {
// When we've been at maximum speed for the given duration, start
// to decelerate.
if (++warpFrames >= BOOST_FRAMES) {
warpFrames = 0;
accelerating = FALSE;
}
// Otherwise, if we are in the deceleration phase of warping, slow down
// the camera until it is at minimum speed.
} else if (!accelerating && decelerate(&speed, BOOST_ACCEL)) {
// When we've been at minimum speed for the given duration, start
// to accelerate.
if (++warpFrames >= BOOST_FRAMES) {
warpFrames = 0;
accelerating = TRUE;
}
}
// Now loop through each star again, to update their positions and draw
// them to the display.
for (i = 0; i < STAR_COUNT; ++i) {
stars[i].z -= speed;
stars[i].x -= strafeSpeed;
// If the star is too far to the left, move it right.
if (stars[i].x < -0.5f) {
stars[i].x += 1.0f;
}
// If the star is too far to the right, move it left.
if (stars[i].x >= 0.5f) {
stars[i].x -= 1.0f;
}
// If the star is behind the camera, push it to the back of the
// scene and randomize its X and Y position.
if (stars[i].z <= 0.0f) {
stars[i].z = 1.0f;
randomizeStar(&stars[i]);
}
// Draw the star in white.
renderStar(stars[i], speed, stars[i].clr);
}
// Ease smoothSpeed towards the current value of speed.
smoothSpeed += (speed - smoothSpeed) * 0.1f;
// Draw the speed bar things on either side of the display.
renderSpeedBar(4, 4, 6, DISPLAY_HEIGHT - 8,
smoothSpeed, warping ? YELLOW : WHITE);
renderSpeedBar(DISPLAY_WIDTH - 10, 4, 6, DISPLAY_HEIGHT - 8,
smoothSpeed, warping ? YELLOW : WHITE);
// I think we have some time to spare.
wait(16);
}
// This should never happen.
return (int) (1.0 / 0.0);
}
Car::Car() : color(Qt::green), wheelsAngle(0), speed(0)
{
if(QCoreApplication::arguments().size() >= 3)
{
m_pCarControllerInterProcessSignalPropagator = new CCarControllerInterProcessSignalPropagator(QInterProcessSignalPropogator::InterProcessSignalPropogatorServer, this, QCoreApplication::arguments().at(1), QCoreApplication::arguments().at(2).toInt());
}
else
{
m_pCarControllerInterProcessSignalPropagator = new CCarControllerInterProcessSignalPropagator(QInterProcessSignalPropogator::InterProcessSignalPropogatorServer, this);
}
bool bRetvalue = false;
bRetvalue = connect(m_pCarControllerInterProcessSignalPropagator, SIGNAL(signalAccelerate()), this, SLOT(accelerate()), Qt::QueuedConnection);
bRetvalue = connect(m_pCarControllerInterProcessSignalPropagator, SIGNAL(signalDecelerate()), this, SLOT(decelerate()), Qt::QueuedConnection);
bRetvalue = connect(m_pCarControllerInterProcessSignalPropagator, SIGNAL(signalTurnLeft()), this, SLOT(turnLeft()), Qt::QueuedConnection);
bRetvalue = connect(m_pCarControllerInterProcessSignalPropagator, SIGNAL(signalTurnRight()), this, SLOT(turnRight()), Qt::QueuedConnection);
startTimer(1000 / 33);
setFlag(QGraphicsItem::ItemIsMovable, true);
setFlag(QGraphicsItem::ItemIsFocusable, true);
}
void MyPlane::move(double dt)
{
/// Zmienna skaluj¹ca prêdkoœæ
double vscale = 100;
height = abs(position.y - 2160)*7.5;
/// Obliczenie prêdkoœci wzglêdem wiatru
if (windDirection >= -90 && windDirection <= 90)
{
v = (velocity - windSpeed) * 1000 / 3600;
}
else if ((windDirection >= -180 && windDirection < -90) || (windDirection <= 180 && windDirection>90))
{
v = (velocity + windSpeed) * 1000 / 3600;
}
/// Wyliczenie wartoœci si³
gravity = 9.81;
Pz = 0.5*Cz*S*ro*v*v;
Fg = mass*gravity;
/// Wyliczenie wartoœci prêdkoœci w osi y
speed.y += (Pz - Fg)*dt / mass;
/// Podejscie do l¹dowania, gdy stan paliwa spadnie poni¿ej alarmowego
if (fuel < 35000 && fuelalarm == false)
{
cout << "\r" << "Malo paliwa! Podchodze do ladowania!";
state = State::DESCENDING;
fuelalarm = true;
}
/// Logika, w przypadku wznoszenia
if (state == State::ASCENDING)
{
if (rotation > -10) rotation -= 0.05;
if (Pz < Fg)
{
accelerate(1);
}
if (Pz >= Fg && velocity < 900 && position.y>800)
{
accelerate(0.25);
}
if (position.y <= 800)
{
state = State::MAINTAINING;
}
else
{
position.x += velocity*cos((rotation * 3.14) / 180.0f)*dt;
position.y += speed.y*sin((rotation * 3.14) / 180.0f)*dt;
}
}
/// Logika w przypadku schodzenia
if (state == State::DESCENDING)
{
/// Wytraca prêdkoœæ
if (Pz > 5 * Fg)
{
decelerate(0.25);
if (rotation < 1) rotation += 0.05;
if (rotation > 1) rotation -= 0.05;
}
/// Podchodzi do l¹dowania
if (Pz <= 5 * Fg)
{
if (rotation < 3) rotation += 0.05;
if (rotation > 3) rotation -= 0.05;
if (velocity > 400) decelerate(0.25);
}
if (position.y > 1700)
{
state = State::MAINTAINING;
fuelalarm = false;
}
position.x += velocity*cos((rotation * 3.14) / 180.0f)*dt;
position.y += speed.y*sin((rotation * 3.14) / 180.0f)*dt;
}
/// Logika w przypadku lotu na sta³ej wysokoœci
if (state == State::MAINTAINING)
{
if (fuel < 35000 && fuelalarm == true)
{
cout << "\r" << "Malo paliwa! Podchodze do ladowania!";
state = State::DESCENDING;
}
if (rotation < 0) rotation += 0.05;
if (rotation > 0) rotation -= 0.05;
if (position.y >= 1700) state = State::LANDING;
position.x += velocity*cos((rotation * 3.14) / 180.0f)*dt;
position.y += speed.y*sin((rotation * 3.14) / 180.0f)*dt;
}
if (position.y < 600) position.y = 600;
if (position.y > 2100)
{
position.y = 2100;
}
if (position.x >= 1920)
position.x = 640;
if (position.x < 640)
position.x = 1920;
distance += velocity / 10 * dt;
}
void Player::action(Event event)
{
if (event == DECELERATE) {
decelerate();
}
}
