void runMotorSpeeds(int &motorSpeedD, int &motorSpeedE, int &motorSpeedF, int &motorSpeedG, int angle, int Vb) {
float Vw1, Vw2, Vw3, Vw4, norm_factor;
Vw1 = Vb*cosDegrees(angle);
Vw2 = Vb*sinDegrees(angle);
Vw3 = -Vw1;
Vw4 = -Vw2;
norm_factor = 1.0;
if (Vw1 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw1;
} else if (Vw2 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw2;
} else if (Vw3 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw3;
} else if (Vw4 > MAXMOTORSPEED) {
norm_factor = MAXMOTORSPEED / Vw4;
}
motorSpeedD = roundit(Vw1 * norm_factor);
motorSpeedE = roundit(Vw2 * norm_factor);
motorSpeedF = roundit(Vw3 * norm_factor);
motorSpeedG = roundit(Vw4 * norm_factor);
motor[motorD] = motorSpeedD;
motor[motorE] = motorSpeedE;
motor[motorF] = motorSpeedF;
motor[motorG] = motorSpeedG;
}
dtype
fn(type x)
{
if (INRANGE(x)) {
x = roundit(x);
return ((dtype)x);
} else {
feraiseexcept(FE_INVALID);
return (DTYPE_MAX);
}
}
/*
* C99 says we should not raise a spurious inexact exception when an
* invalid exception is raised. Unfortunately, the set of inputs
* that overflows depends on the rounding mode when 'dtype' has more
* significant bits than 'type'. Hence, we bend over backwards for the
* sake of correctness; an MD implementation could be more efficient.
*/
dtype
fn(type x)
{
fenv_t env;
dtype d;
feholdexcept(&env);
d = (dtype)roundit(x);
if (fetestexcept(FE_INVALID))
feclearexcept(FE_INEXACT);
feupdateenv(&env);
return (d);
}
void OS_get_table() {
kvm_t *kd;
char errbuf[_POSIX2_LINE_MAX];
struct kinfo_proc *procs; /* array of processes */
int count; /* returns number of processes */
int i, j;
int seconds, minutes, secleft; /* for time[20] */
int milsec, shortmsec; /* for miliseconds of time[20] */
int ttynum;
long start;
char *ttydev;
static time_t now; /* for started[20] */
struct tm *tp; /* for month/day/hour/min/AM/PM fields of started[20] */
int SECSPERHOUR = 3600;
int SECSPERDAY = 24 * 3600;
pid_t sesid;
int length;
char cmndline[MAXARGLN+1];
char ** argv;
/* for bless_into_proc */
static char format[F_LASTFIELD + 1];
/* variables to hold some values for bless_into_proc */
char state[20];
char cputime[20];
char started[20];
char session[20];
char shortsess[20];
/* Open the kvm interface, get a descriptor */
if ((kd = kvm_open(NULL, NULL, NULL, 0, errbuf)) == NULL) {
/* fprintf(stderr, "kvm_open: %s\n", errbuf); */
ppt_croak("kvm_open: ", errbuf);
}
/* Get the list of processes. */
if ((procs = kvm_getprocs(kd, KERN_PROC_ALL, 0, &count)) == NULL) {
kvm_close(kd);
/* fprintf(stderr, "kvm_getprocs: %s\n", kvm_geterr(kd)); */
ppt_croak("kvm_getprocs: ", kvm_geterr(kd));
}
/* Iterate through the processes in kinfo_proc, sending proc info */
/* to bless_into_proc for each proc */
for (i=0; i < count; i++) {
static struct pstats ps;
static struct session seslead;
strcpy(format, Defaultformat);
/* get ttydev */
ttynum=procs[i].kp_eproc.e_tdev;
ttydev=devname(ttynum, S_IFCHR);
if (ttydev == NULL) ttydev = "??";
/* get the state of processes */
switch (procs[i].kp_proc.p_stat)
{
case SIDL:
strcpy(state, IDLE);
break;
case SRUN:
strcpy(state, RUN);
break;
case SSLEEP:
strcpy(state, SLEEP);
break;
case SSTOP:
strcpy(state, STOP);
break;
case SZOMB:
strcpy(state, ZOMBIE);
break;
default:
strcpy(state, UNKNOWN);
break;
}
/* get the cpu time of processes */
seconds=procs[i].kp_proc.p_rtime.tv_sec;
milsec=procs[i].kp_proc.p_rtime.tv_usec;
shortmsec=roundit(milsec);
if (seconds < 60) {
if (seconds < 10)
sprintf(cputime, "0:0%d.%d", seconds, shortmsec);
else
sprintf(cputime, "0:%d.%d", seconds, shortmsec);
}
else {
minutes=seconds/60;
secleft=seconds-(minutes * 60);
if (secleft < 10)
sprintf(cputime, "%d:0%d.%d", minutes, secleft, shortmsec);
else
sprintf(cputime, "%d:%d.%d", minutes, secleft, shortmsec);
}
/* get the start time of process (when started) */
/* fill the pstats struct using kvm_read */
if (kvm_read(kd, (u_long)procs[i].kp_proc.p_stats, (char *)&ps, sizeof(ps)) == sizeof(ps)) {
start=ps.p_start.tv_sec;
tp=localtime(&start);
if (!now)
(void)time(&now);
if (now - ps.p_start.tv_sec < 24 * SECSPERHOUR) {
static char fmt[] = __CONCAT("%l:%", "M%p");
(void)strftime(started, sizeof(started) - 1, fmt, tp);
}
else if (now - ps.p_start.tv_sec < 7 * SECSPERDAY) {
static char fmt[] = __CONCAT("%a%", "I%p");
(void)strftime(started, sizeof(started) - 1, fmt, tp);
}
else
(void)strftime(started, sizeof(started) - 1, "%e%b%y", tp);
}
/* get the session ID (ie: session pointer ID) */
sprintf(session, "%x", (u_long)procs[i].kp_eproc.e_sess);
length=strlen(session);
for (j=0; j < length; j++) {
if (session[1] == '1')
shortsess[j]=session[j+1];
else
shortsess[j]=session[j+2];
}
/* fill the session leader and proc struct using kvm_read */
if (kvm_read(kd, (u_long)procs[i].kp_eproc.e_sess, (char *)&seslead, sizeof(seslead)) == sizeof(seslead)) {
static struct proc leader;
if (kvm_read(kd, (u_long)seslead.s_leader, (char *)&leader, sizeof(leader)) == sizeof(leader)) {
sesid=leader.p_pid;
}
}
/* retrieve the arguments */
cmndline[0] = '\0';
argv = kvm_getargv(kd, (const struct kinfo_proc *) &(procs[i]) , 0);
if (argv) {
int j = 0;
while (argv[j] && strlen(cmndline) <= MAXARGLN) {
strcat(cmndline, argv[j]);
strcat(cmndline, " ");
j++;
}
}
/* access everything else directly from the kernel, send it */
/* into bless_into_proc */
bless_into_proc( format,
Fields,
procs[i].kp_eproc.e_pcred.p_ruid,
procs[i].kp_eproc.e_pcred.p_rgid,
procs[i].kp_proc.p_pid,
procs[i].kp_eproc.e_ppid,
procs[i].kp_eproc.e_pgid,
procs[i].kp_proc.p_priority - PZERO,
shortsess,
sesid,
cputime,
procs[i].kp_eproc.e_wmesg,
procs[i].kp_proc.p_comm,
state,
started,
ttydev,
ttynum,
cmndline
);
}
if (kd) {
kvm_close(kd);
}
}
int main(int argc, char *argv[])
{
// Local variable declaration
double t0 = 0; // Intial time , mostly 0
double x0 = 0; // Initial mean number of customers at time t0
double h = 1;
double x1 = 0;
int i, opt, stable_cnt = 0, n, pflag = 0;
if (argc != 9 && argc != 11) {
printf("argc %d\n", argc);
usage(argv[0]);
return 1;
}
while ((opt = getopt(argc, argv, ":h:x:l:u:p:")) != -1) {
//printf("opt %c optarg %s\n", opt, optarg);
switch (opt) {
case 'h':
h = strtod(optarg, NULL);
//printf("optarg %s h = %lf\n", optarg, h);
break;
case 'x':
x0 = strtod(optarg, NULL);
//printf("optarg %s x0 = %lf\n", optarg, x0);
//printf("val = %lf\n", strtod(optarg, NULL));
break;
case 'l':
lambda = strtod(optarg, NULL);
//printf("optarg %s lambda = %f\n", optarg, lambda);
//printf("val = %lf\n", strtod(optarg, NULL));
break;
case 'u':
mu = strtod(optarg, NULL);
//printf("optarg %s mu = %f\n", optarg, mu);
//printf("val = %lf\n", strtod(optarg, NULL));
break;
case 'p':
pflag = 1;
n = atoi(optarg);
break;
default:
usage(argv[0]);
return 1;
}
}
//lambda = 0.8; mu = 1.0; x0 = 0; h = 0.1;
printf("# Average number of pkts in the system as a function of time t\n");
printf("# Parameters for this plot\n");
printf("# lambda = %lf mu = %f h = %f x0 = %f\n", lambda, mu, h, x0) ;
printf("#\n");
if (pflag) {
printf("# time \t\tAvg-number-of-pkts\t\t P(n=%d)\n", n);
} else {
printf("# time \t\tAvg-number-of-pkts\n");
}
if (pflag) {
printf("%lf\t\t%.7f\t\t%lf\n", t0, x0, find_pn(x0, n));
} else {
printf("%lf\t\t%.7f\n", t0, x0);
}
/*
* This loop continues to calculate until the value of x stays stable for
* STABLE_CNT_MAX iterations or upto MAX_ITR iterations.
*/
for(i = 0; i < MAX_ITR; i++) {
x1 = calc_next_value(t0, x0, h);
if (pflag) {
printf("%lf\t\t%.7f\t\t%lf\n", t0, x1, find_pn(x1, n));
} else {
printf("%lf\t\t%.7f\n", t0, x1);
}
//printf("iter %d x0 = %f x1 = %f stable_cnt = %d\n", i, x0, x1, stable_cnt);
if (roundit(x0) == roundit(x1)) {
stable_cnt++;
if (stable_cnt >= STABLE_CNT_MAX) {
printf("Breaking early...\n");
break;
}
} else {
/*
* The value of x has not yet stabilized hence we reset the count.
*/
stable_cnt = 0;
}
x0 = x1;
t0 = t0 + h;
}
return 0;
}
