Eigen::Vector3d range_bearing_elevation::to_cartesian() const
{
double xy_projection( range() * std::cos( elevation() ) );
return ::Eigen::Matrix< double, 3, 1 >( xy_projection * std::cos( bearing() )
, xy_projection * std::sin( bearing() )
, range() * std::sin( elevation() ) );
}
double bearing_elevation::elevation( double t )
{
double e( mod_( t, ( double )( M_PI * 2 ) ) ); //double e( std::fmod( t, ( double )( M_PI * 2 ) ) );
if( comma::math::less( e, 0 ) ) { e += M_PI * 2; }
if( !comma::math::less( e, M_PI / 2 ) )
{
if( !comma::math::less( e, M_PI ) )
{
if( !comma::math::less( e, M_PI * 3 / 2 ) )
{
e = e - M_PI * 2;
}
else
{
e = M_PI - e;
bearing( bearing() + M_PI );
}
}
else
{
e = M_PI - e;
bearing( bearing() + M_PI );
}
}
elevation_ = e;
return elevation_;
}
bool
PointFeatureObs::isSane() const
{
if ( !hydrofeatureobs::isSane( pFalsePositive(), pTruePositive() ) ) return false;
if ( range() < 0 ) return false;
if ( bearing() < -M_PI || bearing() > M_PI ) return false;
if ( rangeSd() < 0 || bearingSd() < 0 ) return false;
return true;
}
// crosstrack distance in metres from point3 to the point1-point2 line
double crosstrack(double lat1, double lon1, double lat2, double lon2, double lat3, double lon3)
{
const double R = 6371E3; // m
double distance13 = distance(lat1, lon1, lat3, lon3) / R;
double bearing13 = bearing(lat1, lon1, lat3, lon3);
double bearing12 = bearing(lat1, lon1, lat2, lon2);
return asin(sin(distance13) * sin(bearing13 - bearing12)) * R;
}
qreal AlternativeRoutesModelPrivate::distance( const GeoDataCoordinates &satellite, const GeoDataCoordinates &lineA, const GeoDataCoordinates &lineB )
{
qreal dist = distanceSphere( lineA, satellite );
qreal bearA = bearing( lineA, satellite );
qreal bearB = bearing( lineA, lineB );
qreal result = asin( sin ( dist ) * sin( bearB - bearA ) );
Q_ASSERT( qMax<qreal>( distanceSphere(satellite, lineA), distanceSphere(satellite, lineB) ) >= qAbs<qreal>(result) );
result = acos( cos( dist ) / cos( result ) );
/** @todo: This is a naive approach. Look into the maths. */
qreal final = qMin<qreal>( distanceSphere( satellite, lineA ), distanceSphere( satellite, lineB ) );
if ( result >= 0 && result <= distanceSphere( lineA, lineB ) ) {
GeoDataCoordinates nearest = coordinates( lineA, result, bearB );
return qMin<qreal>( final, distanceSphere( satellite, nearest ) );
} else {
/*
* returns the number of banks we're going to fire
* it also sets them up to fire.
*/
int
e_phasers(struct ship *sp, struct ship *fed)
{
int i;
int banks;
int hit;
int howmany;
float bear;
banks = 0;
howmany = randm(sp->num_phasers / 2) + sp->num_phasers / 2;
sp->p_spread = 10 + randm(12);
for (i=0; i<sp->num_phasers; i++) {
if ((sp->phasers[i].status & P_DAMAGED) ||
(sp->phasers[i].load == 0))
continue;
if (fed != NULL) {
if (sp->phasers[i].target == NULL)
continue;
bear = bearing(sp->x, fed->x, sp->y, fed->y);
hit = phaser_hit(sp, fed->x, fed->y, &sp->phasers[i], bear);
if (hit <= 0)
continue;
}
banks++;
sp->phasers[i].status |= P_FIRING;
if (banks >= howmany)
break;
}
return banks;
}
/*
* This routine will turn the ship, slowing down if necessary to
* facilitate the turn. (Always returns 1)
*/
int
e_pursue(struct ship *sp, struct ship *fed, float speed)
{
float bear;
float coursediff;
bear = bearing(sp->x, fed->x, sp->y, fed->y);
/*
* do a quick turn if our speed is > max_warp - 2 and
* (thus) we are never going to bear on the fed ship
* speed = max_warp / 2 is a magic cookie. Feel free to change.
*/
coursediff = rectify(sp->course - bear);
if (coursediff > 180.0)
coursediff -= 360.0;
if (speed >= sp->max_speed - 2 && fabs(coursediff) > 10)
speed = (int)(sp->max_speed / 2);
sp->target = fed;
sp->newcourse = bear;
sp->newwarp = speed;
if (speed > 1 && is_dead(sp, S_WARP))
sp->newwarp = 0.99;
#ifdef TRACE
if (trace) {
printf("*** Pursue: Newcourse = %.2f\n", sp->newcourse);
printf("*** Pursue: Newwarp = %.2f\n", sp->newwarp);
}
#endif
return 1;
}
QcPolygon
QcViewport::compute_polygon() const
{
QcVectorDouble center = projected_center_coordinate();
// QcInterval2DDouble new_area = interval_from_center_and_radius(center, m_half_diagonal_m);
// Fixme: cache more ?
// Fixme: anti-clockwise ???
QcVectorDouble point1 = center + m_half_diagonal_m;
QcVectorDouble point2 = center + m_half_diagonal_m.mirror_x();
QcVectorDouble point3 = center + m_half_diagonal_m.rotate_180();
QcVectorDouble point4 = center + m_half_diagonal_m.mirror_y();
QcPolygon polygon; // = {}
polygon.add_vertex(point3);
polygon.add_vertex(point2);
polygon.add_vertex(point1);
polygon.add_vertex(point4);
// Bearing: make only sense for local map, with eventually one wrapping
double _bearing = bearing();
if (_bearing)
polygon = polygon.rotate_counter_clockwise(_bearing); // Fixme: copy ?
return polygon;
}
std::string
PoseFeatureObs::toString() const
{
stringstream ss;
ss << "PSE [r="<<range()<<", b="<<bearing()*180.0/M_PI<<"deg, o="<<orientation()*180.0/M_PI<<"deg]"
<< "(t="<<featureType()<<",pf="<<pFalsePositive()<<",pt="<<pTruePositive()<<",sd="<<rangeSd()<<","<<bearingSd()*180.0/M_PI<<"deg"<<orientationSd()*180.0/M_PI<<"deg)";
return ss.str();
}
QString OsmDatabase::formatDistance( const GeoDataCoordinates &a, const GeoDataCoordinates &b )
{
qreal distance = EARTH_RADIUS * distanceSphere( a, b);
int precision = 0;
QString distanceUnit = QLatin1String( "m" );
if ( MarbleGlobal::getInstance()->locale()->measurementSystem() == MarbleLocale::ImperialSystem ) {
precision = 1;
distanceUnit = "mi";
distance *= METER2KM;
distance *= KM2MI;
} else if (MarbleGlobal::getInstance()->locale()->measurementSystem() ==
MarbleLocale::MetricSystem) {
if ( distance >= 1000 ) {
distance /= 1000;
distanceUnit = "km";
precision = 1;
} else if ( distance >= 200 ) {
distance = 50 * qRound( distance / 50 );
} else if ( distance >= 100 ) {
distance = 25 * qRound( distance / 25 );
} else {
distance = 10 * qRound( distance / 10 );
}
} else if (MarbleGlobal::getInstance()->locale()->measurementSystem() ==
MarbleLocale::NauticalSystem) {
precision = 2;
distanceUnit = "nm";
distance *= METER2KM;
distance *= KM2NM;
}
QString const fuzzyDistance = QString( "%1 %2" ).arg( distance, 0, 'f', precision ).arg( distanceUnit );
int direction = 180 + bearing( a, b ) * RAD2DEG;
QString heading = QObject::tr( "north" );
if ( direction > 337 ) {
heading = QObject::tr( "north" );
} else if ( direction > 292 ) {
heading = QObject::tr( "north-west" );
} else if ( direction > 247 ) {
heading = QObject::tr( "west" );
} else if ( direction > 202 ) {
heading = QObject::tr( "south-west" );
} else if ( direction > 157 ) {
heading = QObject::tr( "south" );
} else if ( direction > 112 ) {
heading = QObject::tr( "south-east" );
} else if ( direction > 67 ) {
heading = QObject::tr( "east" );
} else if ( direction > 22 ) {
heading = QObject::tr( "north-east" );
}
return fuzzyDistance + QLatin1Char(' ') + heading;
}
/*
* This routine has the enemy ship turn its strongest shield towards
* the enemy and then accelerate to 2/3 maximum speed. (Always
* returns 1)
*/
int
e_runaway(struct ship *sp, struct ship *fed)
{
double bear;
int strong;
double strength;
double temp;
int sign = 1;
float course = 0.0;
int i;
bear = bearing(sp->x, fed->x, sp->y, fed->y);
/*
* Find the strongest shield
*/
strong = 0;
strength = 0.;
for (i=0; i< SHIELDS; i++) {
temp = sp->shields[i].eff * sp->shields[i].drain *
(i == 0 ? SHIELD1 : 1.);
if (temp > strength) {
strong = i;
strength = temp;
}
}
switch (strong) {
case 0: course = bear;
sign = -1;
break;
case 1: course = rectify(bear - 90);
sign = 1;
break;
case 2: course = rectify(bear + 180);
sign = 1;
break;
case 3: course = rectify(bear + 90);
sign = 1;
break;
}
sp->target = NULL;
sp->newcourse = course;
sp->newwarp = 2 / 3 * sp->max_speed * sign;
if (sp->newwarp > 1.0 && is_dead(sp, S_WARP))
sp->newwarp = 0.99;
if (cansee(sp) && syswork(fed, S_SENSOR))
printf("%s: The %s is retreating.\n", helmsman, sp->name);
#ifdef TRACE
if (trace) {
printf("*** Runaway: Newcourse = %.2f\n", sp->newcourse);
printf("*** Runaway: Newwarp = %.2f\n", sp->newwarp);
}
#endif
return 1;
}
double great_circle::arc::angle_towards( const coordinates &c ) const
{
if( has(c) ) { return 0; }
// todo: using bearing may be very inaccurate; simply turning the arc axis should be the right way, literally something like this:
// Eigen::Quaternion< double >::FromTwoVectors( axis(), great_circle::arc( begin_coordinates_, c ).axis() ).angle()
double result = bearing(0) - arc(begin_coordinates_, c).bearing(0);
if (result > M_PI) { result = -2*M_PI + result; }
return result;
}
varargs int AddMomentum(object what, object from, object to, int speed){
int x1, y1, z1, x2, y2, z2;
int a, b;
if(previous_object() != what) return 0;
if(from == to) return 0;
if(!from || !to || !speed) return 0;
a = sscanf(ROOMS_D->GetCoordinates(from,"%d,%d,%d",x1,y1,z1);
b = sscanf(ROOMS_D->GetCoordinates(to,"%d,%d,%d",x2,y2,z2);
if(a < 3 || b < 3) return 0;
if(x1 == x2 && y1 == y2 && z1 == z2) return 0;
if(!Momentum[what]) Momentum[what] = ([ "dir" : 0, "speed" : 0 ]);
newdir = bearing(x1, y1, x2, y2);
olddir = Momentum[what]["dir"];
oldspeed = Momentum[what]["speed"];
void QQuickMapboxGLRenderer::synchronize(QQuickFramebufferObject *item)
{
if (!m_initialized) {
auto qquickMapbox = static_cast<QQuickMapboxGL*>(item);
QObject::connect(m_map.data(), &QMapboxGL::needsRendering, qquickMapbox, &QQuickMapboxGL::update);
QObject::connect(this, &QQuickMapboxGLRenderer::centerChanged, qquickMapbox, &QQuickMapboxGL::setCenter);
m_initialized = true;
}
auto quickMap = static_cast<QQuickMapboxGL*>(item);
auto syncStatus = quickMap->swapSyncState();
if (syncStatus & QQuickMapboxGL::CenterNeedsSync || syncStatus & QQuickMapboxGL::ZoomNeedsSync) {
const auto& center = quickMap->center();
m_map->setCoordinateZoom({ center.latitude(), center.longitude() }, quickMap->zoomLevel());
}
if (syncStatus & QQuickMapboxGL::StyleNeedsSync) {
m_map->setStyleURL(quickMap->style());
}
if (syncStatus & QQuickMapboxGL::PanNeedsSync) {
m_map->moveBy(quickMap->swapPan());
emit centerChanged(QGeoCoordinate(m_map->latitude(), m_map->longitude()));
}
if (syncStatus & QQuickMapboxGL::BearingNeedsSync) {
m_map->setBearing(quickMap->bearing());
}
if (syncStatus & QQuickMapboxGL::PitchNeedsSync) {
m_map->setPitch(quickMap->pitch());
}
if (syncStatus & QQuickMapboxGL::ColorNeedsSync && m_map->isFullyLoaded()) {
m_map->setPaintProperty("background", "background-color", quickMap->color());
}
}
/*
* Returns 1 if a probe was launched
*/
int
e_launchprobe(struct ship *sp, struct ship *fed)
{
int i;
struct list *lp;
struct torpedo *tp;
if (!syswork(sp, S_PROBE) || sp->energy <= 10 || cantsee(sp))
return 0;
/*
* fed ship has to be going slow before we'll launch
* a probe at it.
*/
if (fabs(fed->warp) > 1.0)
return 0;
lp = newitem(I_PROBE);
tp = lp->data.tp = MKNODE(struct torpedo, *, 1);
if (tp == (struct torpedo *)NULL) {
fprintf(stderr, "e_launchprobe: malloc failed\n");
exit(2);
}
tp->from = sp;
tp->speed = sp->warp;
tp->newspeed = 3.0;
tp->target = fed;
tp->course = bearing(sp->x, fed->x, sp->y, fed->y);
tp->x = sp->x;
tp->y = sp->y;
tp->prox = 200 + randm(200);
tp->timedelay = 15.;
tp->id = new_slot();
if ((i = randm(15) + 10) > sp->energy)
i = sp->energy;
tp->fuel = i;
tp->type = TP_PROBE;
sp->energy -= i;
sp->pods -= i;
printf("%s launching probe #%d\n", sp->name, tp->id);
return 1;
}
/*
* Advance to the rear! (Always returns 1)
*/
int
e_evade(struct ship *sp, int x, int y, int type)
{
float newcourse = 0.0;
float bear;
bear = bearing(sp->x, x, sp->y, y);
if (cansee(sp) && syswork(shiplist[0], S_SENSOR))
printf("%s taking evasive action!\n", sp->name);
switch (randm(3)) {
case 1:
newcourse = rectify(bear - 90.0);
break;
case 2:
newcourse = rectify(bear + 90.0);
break;
case 3:
newcourse = rectify(bear + 180.0);
break;
default:
printf("error in evade()\n");
break;
}
sp->target = NULL;
sp->newcourse = newcourse;
sp->newwarp = 2 + randm((int)(sp->max_speed - 3));
if (is_dead(sp, S_WARP))
sp->newwarp = 1.0;
#ifdef TRACE
if (trace) {
printf("*** Evade: Newcourse = %3.0f\n", newcourse);
printf("*** Evade: Newwarp = %.2f\n", sp->newwarp);
}
#endif
type = type; /* LINT */
return 1;
}
SEXP bearing(SEXP x0, SEXP y0, SEXP x1, SEXP y1, SEXP nautical){
return asSEXP(bearing(asDouble(x0),asDouble(y0),asDouble(x1),asDouble(y1),asBool(nautical)));
}
int main ( void )
{
//TRISA = 0b0010000000000000;
TRISA = 0b00001111;
TRISAbits.TRISA12 = 0;
TRISCbits.TRISC1 = 0;
TRISCbits.TRISC2 = 0;
TRISCbits.TRISC3 = 0;
TRISCbits.TRISC4 = 0;
//TRISD = 0;
TRISD = 0b0000000000010000; // RD11: DIR2; RD8: PWM2
//TRISE = 0b00001000;
TRISE = 0b00001000; // RE6: DIR1; RE0: PWM1
TRISEbits.TRISE7 = 0;
//LCD an
_PCFG16 = 1; // AN16 is digital
_PCFG17 = 1; // AN17 is digital
_PCFG18 = 1; // AN18 is digital
_PCFG19 = 1; // AN19 is digital
_PCFG20 =1;
_PCFG31=1;
//pwm analog select
_PCFG24=1;
_PCFG30=1;
//RE4 for lcd r/s
_PCFG28=1;
TRISEbits.TRISE4 = 0;
AD1PCFGHbits.PCFG28 = 1;
AD1PCFGHbits.PCFG27 = 1;
AD1PCFGHbits.PCFG30 = 1;
AD1PCFGHbits.PCFG24 = 1;
AD1PCFGH = 0x0020;
TRISAbits.TRISA13 = 1;
//set pwm low
PWM1=0;
PWM2=0;
SYS_Initialize ( ) ;
CLKDIVbits.FRCDIV = 0;
CLKDIVbits.PLLPOST = 0; // N2 = 2
CLKDIVbits.PLLPRE = 0; // N1 = 2
PLLFBD = (Fosc*N1_default*N2_default/Ffrc) - M_default; // M = 8 -> Fosc = 7.3728 MHz * 8 / 2 / 2 = 16 MHz
while(!OSCCONbits.LOCK); // Wait for PLL to lock
RCONbits.SWDTEN = 0; // Disable Watch Dog Timer
//TRISF = 0;
gpsLock = 0;
lcd_init();
print_lcd("Initializing");
//delay_ms(500);
//lcd_clear();
/*lcd test
char line1[] = " On Route ";
char line2[] = " Arrived ";
//send_command_byte(0xFF);
//while(1){send_command_byte(0xFF);send_data_byte(0);}
// delay_ms(2);
//send_command_byte(0x02); // Go to start of line 1
//send_command_byte(0b00001111); // Display: display on, cursor on, blink on
//send_command_byte(0b00000110); // Set entry mode: ID=1, S=0
//send_command_byte(0b00000001); // Clear display
print_lcd(line1);
delay_ms(5000);
lcd_clear();
print_lcd(line2);
//send_command_byte(0xC0); // Go to start of line 1
//while(1){send_command_byte(0b00000001);}
while(1);*/
/*int a;
long long int ct;
int i;
int j=0;*/
//i2c init
//uart init
UART_Initialize();
delayMs(100);
/* while(1){//test for delay ms configuration
//PORTDbits.RD1 = 1;
//delayUs(10);
//for(i = 0;i <7;i++);
delayMs(10);
PORTDbits.RD12 = 1;
//for (i = 0; i < 1000; i++);
//PORTDbits.RD1 = 0;
//for(i = 0;i <7;i++);
delayMs(10);
PORTDbits.RD12 = 0;
//for (i = 0; i < 1000; i++);
}*/
//ultrasonic test
/* while(1){
long double x;
delayMs(500);
PORTEbits.RE4 = 1; //TRIGGER HIGH
PORTDbits.RD5 = 1;
delay_us(10); //10uS Delay
lcd_clear();
ultraSonicEn = 1;
ultraSonicCount = 0;
PORTEbits.RE4 = 0; //TRIGGER LOW
PORTDbits.RD5 = 0;
while (!PORTDbits.RD4); //Waiting for Echo
IEC0bits.T2IE = 1; //enable timer
while(PORTDbits.RD4);//
IEC0bits.T2IE = 0; //disable timer
x = ultraSonicCount/TICKS_PER_METER;
sprintf(outputBuf,"%lf",x);
print_lcd(outputBuf);
}*/
//TX_str(endGPS);
//delayMs(3000);
//TX_str(startGPS);
/*TX_str(startGPShot);
delayMs(2000);
while ( !gpsLock ) {
TX_str(getGPS);
delayMs(500);
}*/
//TCP code
TX_str(openNet);
delayMs(5000);
TX_str(openConnection);
delayMs(5000);
// while(!BUTTON_IsPressed ( BUTTON_S3 ));
//TX_str(sprintf("%s%d\r",sendTCP, strlen("10")));
sprintf(outputBuf2,"2\n%lf,%lf\n\r",roverLog,roverLat );
sprintf(cmdBuf,"%s%d\r", sendTCP,strlen(outputBuf2));
TX_str(cmdBuf);
delayMs(3000);
TX_str(outputBuf2);
while(!waypointsReady || !gpsLock);
//while(1);
delayMs(7000);
lcd_clear();
print_lcd("waypoints locked");
i2c_init();
//i2c_write3(0x3c, 0x00, 0x70);
i2c_write3(0x3c, 0x00, 0b00011000);
//i2c_write3(0x3c, 0x01, 0b11000000);
i2c_write3(0x3c, 0x01, 0xA0);
i2c_write3(0x3c, 0x02, 0x00);
//timer init, do this after other initializations
motorDuty=0;
tim1_init();
tim2_init();
/*
double angleTolerance = 8.0;
motorDuty=2;
while (1){ //adjust
double angleDiff = headingDiff(0,roverHeading );
if (angleDiff > 0 ||abs(angleDiff) > 175 ){ //turn left
PWM1_DIR = 0; //left NOTE: 0 is forward, 1 is reverse
PWM2_DIR = 1; //right
} else{ // turn right
PWM1_DIR = 1;
PWM2_DIR = 0;
}
if (abs(angleDiff) < angleTolerance){
motorDuty = 0;
//break;
}else{
//motorDuty =4;
motorDuty =2;
}
delayMs(13);
updateHeading();
}*/
//hardcoded gps for tcpip test
/*while(1){
// PORTDbits.RD5 = 1;
// for (a = 0; a < (100/33); a++) {}
// PORTDbits.RD5 = 0;
// for (a = 0; a < (100/33); a++) {}
ct = 0;
//TMR2 = 0;//Timer Starts
delayMs(60);
PORTEbits.RE4 = 1; //TRIGGER HIGH
PORTDbits.RD5 = 1;
delay_us(15);
//10uS Delay
PORTEbits.RE4 = 0; //TRIGGER LOW
PORTDbits.RD5 = 0;
while (!PORTDbits.RD4){ //Waiting for Echo
ct++;
}
//T2CONbits.TON = 1;
while(PORTDbits.RD4) {
ct++;
}// {
//T2CONbits.TON = 0;
sprintf(outputBuf,"%lld", ct);
//a = TMR2;
//a = a / 58.82;
//long int p;
//for(p = 0; p <100000; p++);
}*/
//tim1_init();
//while ( 1 );
//UART_Initialize();
/* Infinite Loop */
/*
long int i;
while ( 1 )
{//test for delay ms configuration
//PORTDbits.RD1 = 1;
delayMs(10);
PORTDbits.RD12 = 1;
//for (i = 0; i < 1000; i++);
//PORTDbits.RD1 = 0;
delayMs(10);
PORTDbits.RD12 = 0;
//for (i = 0; i < 1000; i++);
}*/
//IEC0bits.T1IE = 0;
//IEC0bits.T1IE = 1;
//motorDuty =2;
motorStopFlag = 0; //ready to go
char tempBuf1[50];
int wapointsVisited;
double desiredHeading = 0;
double dToWaypoint = 99999.9;
double angleTolerance = 6.0;
for (wapointsVisited =0; wapointsVisited<numGPSpoints;wapointsVisited++ )
{
dToWaypoint = 99999.9;
while (1)
{
int f;
roverLog = 0;
roverLat = 0;
for(f = 0; f < ROVER_LEN; f++){
roverLog+=roverlog[f]/ROVER_LEN_D;
roverLat+=roverlat[f]/ROVER_LEN_D;
}
dToWaypoint = dist(convertGPSToDeg(roverLat),convertGPSToDeg(roverLog),convertGPSToDeg(gpsLat[wapointsVisited]),convertGPSToDeg(gpsLonge[wapointsVisited]));
if(dToWaypoint <= 3.0){break;} //go to next waypoint
desiredHeading = 330.0;
desiredHeading = bearing(convertGPSToDeg(roverLat),convertGPSToDeg(roverLog),convertGPSToDeg(gpsLat[wapointsVisited]),convertGPSToDeg(gpsLonge[wapointsVisited]));
updateHeading();
if (!(abs(headingDiff(desiredHeading,roverHeading )) < angleTolerance))
{
motorDuty = 0; //stop
delayMs(300);
while (1) //ADJUSTMENT LOOP
{
double angleDiff = headingDiff(desiredHeading,roverHeading );
//double dist = ultraSonicPing();
/*sprintf(tempBuf1,"%f",dist);
lcd_clear();
print_lcd(tempBuf1);*/
if (abs(angleDiff) < angleTolerance){
motorDuty = 0;
/*sprintf(tempBuf1,"%f",roverHeading);
IEC0bits.T1IE = 0;
print_lcd(tempBuf1);
IEC0bits.T1IE = 1;*/
break;
}
if (angleDiff > 0 || abs(angleDiff) > 175 ){ //turn right
/*PORTDbits.RD4 = 1; //right 0 is forwards, 1 i backwards
PORTDbits.RD2 = 1;
PORTDbits.RD8 = 0; //left*/
PWM1_DIR = 0; //left NOTE: 0 is forward, 1 is reverse
PWM2_DIR = 1; //right
} else{ // turn left
/*PORTDbits.RD4 = 0; //right 0 is forwards, 1 i backwards
PORTDbits.RD2 = 0;
PORTDbits.RD8 = 1; //left*/
PWM1_DIR = 1;
PWM2_DIR = 0;
}
motorDuty =3;
delayMs(15);
updateHeading();
// sprintf(tempBuf1,"%f",roverHeading);
// IEC0bits.T1IE = 0;
// lcd_clear();
// print_lcd(tempBuf1);
// IEC0bits.T1IE = 1;
//delayMs(1000);
//for(p = 0; p <100000; p++);
}
}
PWM1_DIR = 0; //left NOTE: 0 is forward, 1 is reverse
PWM2_DIR = 0;
motorDuty = 5;
ultraSonicDelayEnable = 1;
//frequency is around 20kHz
while (ultraSonicDelayCount < 60000){ //for 4 seconds poll ultrasonic and check for obsticles
long double x;
//PORTEbits.RE4 = 1; //TRIGGER HIGH
PORTDbits.RD5 = 1;
delay_us(10); //10uS Delay
lcd_clear();
ultraSonicEn = 1;
ultraSonicCount = 0;
//PORTEbits.RE4 = 0; //TRIGGER LOW
PORTDbits.RD5 = 0;
while (!PORTDbits.RD4); //Waiting for Echo
IEC0bits.T2IE = 1; //enable timer
while(PORTDbits.RD4);//
IEC0bits.T2IE = 0; //disable timer
ultraSonicEn = 0;
x = ultraSonicCount/TICKS_PER_METER;
if (x <= 1.4){
motorDuty = 0;
}else{
motorDuty = 5;
}
delayMs(200);
}
ultraSonicDelayEnable = 0;
ultraSonicDelayCount = 0;
}
motorDuty = 0;
delayMs(1000);
//IEC0bits.T1IE = 0;
lcd_clear();
char buf3[40];
sprintf(buf3, "reached %d", wapointsVisited);
print_lcd(buf3);
delayMs(3000);
//IEC0bits.T1IE = 1;
}
//IEC0bits.T1IE = 0;
lcd_clear();
print_lcd("ARRIVED!!!");
//IEC0bits.T1IE = 1;
while (1);
}
bearing_elevation::bearing_elevation( double b, double e ) : bearing_( bearing( b ) ), elevation_( elevation( e ) ) {}
vector_type to_vector () const { return { x(), y(), bearing() }; }
double bearing_elevation::b() const { return bearing(); }
void ResourceManualFont::deserialization(xml::ElementPtr _node, Version _version)
{
Base::deserialization(_node, _version);
xml::ElementEnumerator node = _node->getElementEnumerator();
while (node.next())
{
if (node->getName() == "Property")
{
const std::string& key = node->findAttribute("key");
const std::string& value = node->findAttribute("value");
if (key == "Source") mSource = value;
else if (key == "DefaultHeight") mDefaultHeight = utility::parseInt(value);
}
}
loadTexture();
if (mTexture != nullptr)
{
int textureWidth = mTexture->getWidth();
int textureHeight = mTexture->getHeight();
node = _node->getElementEnumerator();
while (node.next())
{
if (node->getName() == "Codes")
{
xml::ElementEnumerator element = node->getElementEnumerator();
while (element.next("Code"))
{
std::string value;
// описане глифов
if (element->findAttribute("index", value))
{
Char id = 0;
if (value == "cursor")
id = static_cast<Char>(FontCodeType::Cursor);
else if (value == "selected")
id = static_cast<Char>(FontCodeType::Selected);
else if (value == "selected_back")
id = static_cast<Char>(FontCodeType::SelectedBack);
else if (value == "substitute")
id = static_cast<Char>(FontCodeType::NotDefined);
else
id = utility::parseUInt(value);
float advance(utility::parseValue<float>(element->findAttribute("advance")));
FloatPoint bearing(utility::parseValue<FloatPoint>(element->findAttribute("bearing")));
// texture coordinates
FloatCoord coord(utility::parseValue<FloatCoord>(element->findAttribute("coord")));
// glyph size, default to texture coordinate size
std::string sizeString;
FloatSize size(coord.width, coord.height);
if (element->findAttribute("size", sizeString))
{
size = utility::parseValue<FloatSize>(sizeString);
}
if (advance == 0.0f)
advance = size.width;
GlyphInfo& glyphInfo = mCharMap.insert(CharMap::value_type(id, GlyphInfo(
id,
size.width,
size.height,
advance,
bearing.left,
bearing.top,
FloatRect(
coord.left / textureWidth,
coord.top / textureHeight,
coord.right() / textureWidth,
coord.bottom() / textureHeight)
))).first->second;
if (id == FontCodeType::NotDefined)
mSubstituteGlyphInfo = &glyphInfo;
}
}
}
}
}
}
double bearing_elevation::b( double b ) { return bearing( b ); }
