44 lines
1.8 KiB
C
44 lines
1.8 KiB
C
#include "sim_native_homing.h"
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
|
|
int main(void) {
|
|
native_home *h = malloc(sim_native_home_size());
|
|
native_home *other = malloc(sim_native_home_size());
|
|
double p[12] = {0,0,-1,0,0,0,0,0,-200,200,20,300};
|
|
double position[5] = {10,20,30,15,45};
|
|
assert(sim_native_home_init(h,5,.001) == 0);
|
|
assert(sim_native_home_init(other,5,.001) == 0);
|
|
for (int j=0;j<5;j++) assert(sim_native_home_config(h,j,p) == 0);
|
|
assert(sim_native_home_sync(h,position) == 0);
|
|
assert(sim_native_home_start(h,0,70000) == 0);
|
|
assert(sim_native_home_get(h,0,7) == 10);
|
|
assert(sim_native_home_step(h) == 0);
|
|
assert(sim_native_home_get(h,0,6) == 21);
|
|
assert(sim_native_home_get(h,0,4) == 1);
|
|
assert(sim_native_home_get(h,0,5) == 0);
|
|
assert(sim_native_home_get(h,0,7) == 0);
|
|
assert(sim_native_home_get(h,0,8) == 10);
|
|
assert(sim_native_home_get(h,1,7) == 20);
|
|
for(int i=0;i<10;i++) assert(sim_native_home_step(h) == 0);
|
|
assert(sim_native_home_get(h,0,5) == 1);
|
|
assert(sim_native_home_get(other,0,5) == 0);
|
|
p[1]=5;
|
|
assert(sim_native_home_config(h,1,p) == 0);
|
|
assert(sim_native_home_start(h,1,70001) == 0);
|
|
assert(sim_native_home_step(h) == 0);
|
|
assert(sim_native_home_get(h,1,7)>0 && sim_native_home_get(h,1,7)<5);
|
|
for(int i=0;i<2000 && sim_native_home_get(h,0,0);i++) sim_native_home_step(h);
|
|
assert(fabs(sim_native_home_get(h,1,7)-5)<1e-8);
|
|
assert(fabs(sim_native_home_get(h,1,8)-25)<1e-8);
|
|
assert(sim_native_home_start(h,1,70002) == 0);
|
|
sim_native_home_step(h);
|
|
assert(sim_native_home_cancel(h,0) == 0);
|
|
assert(sim_native_home_get(h,1,5) == 0);
|
|
for (int i=0;i<1000 && sim_native_home_get(h,0,0);i++) sim_native_home_step(h);
|
|
assert(sim_native_home_get(h,0,0) == 0);
|
|
free(h); free(other);
|
|
return 0;
|
|
}
|