//no initialization necessary
BOOLEAN NTAPI InitializeChangeNotify()
{
writeLog(L"Starting HashingPasswordFilter");
if (initializeFilter()){
writeLog(L"HashingPasswordFilter initialized");
return TRUE;
} else {
writeLog(L"HashingPasswordFilter: initialization failed");
return FALSE;
}
}
int main(int argc, const char **argv) {
int i, a;
double t;
int i2c_fd;
coordData_t * waypoints;
int numWaypts = NUM_WAYPOINTS;
double w[] = WAYPOINT_ARRAY;
pthread_t thread;
//Set signals to handle
signal(SIGINT, &sighandle);
signal(SIGTERM, &sighandle);
signal(SIGABRT, &sighandle);
//allocate structs
if((hands = malloc(sizeof(api_HANDLES_t))) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
if((filt = malloc(sizeof(FilterHandles_t))) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
if((pids = malloc(sizeof(pidHandles_t))) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
if((waypoints = malloc(sizeof(coordData_t) * numWaypts)) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
if((posData = malloc(sizeof(roboPos_t))) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
for(i=0; i<numWaypts; i++) {
waypoints[i].X = w[i*2];
waypoints[i].Y = w[i*2+1];
}
// allocate devices
if((hands->c = create_create(COMPORT)) == NULL) {
fprintf(stderr,"Error connecting create\n");
return -1;
}
if((hands->t = turret_create()) == NULL) {
fprintf(stderr,"Error connecting turrett\n");
return -1;
}
//open serial com port
if(create_open(hands->c, FULLCONTROL) < 0) {
fprintf(stderr,"Failed to open create\n");
return -1;
}
//open i2c device
if(turret_open(hands->t) < 0) {
fprintf(stderr,"Failed to connect to robostix\n");
return -1;
}
//initialize the robostix board
turret_init(hands->t);
//Initialize Filter structs
filt->sonar0 = initializeFilter(FILT_SONAR);
filt->sonar1 = initializeFilter(FILT_SONAR);
filt->ir0 = initializeFilter(FILT_IR);
filt->ir1 = initializeFilter(FILT_IR);
//Initialize PID structs
pids->trans = initializePID(TRANS_PID);
pids->angle = initializePID(ANGLE_PID);
pids->sonar = initializePID(SONAR_PID);
pids->angleT = initializePID(ANGLET_PID);
// process command line args
i = 1;
while( i < argc ) {
if(!strcmp(argv[i], "-w")) {
//Accept command line waypoints
if(argc >= i+1) {
numWaypts = atoi(argv[++i]);
if(argc >= i+(2*numWaypts)) {
free(waypoints);
if((waypoints = malloc(sizeof(coordData_t) * numWaypts)) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
for(a=0; a<numWaypts; a++) {
waypoints[a].X = atoi(argv[++i]);
waypoints[a].Y = atoi(argv[++i]);
}
i++;
} else {
printf("Error using [-w].\n\tUsage: -w numPoints X1 Y1 X2 Y2...\n");
}
} else {
printf("Error using [-w].\n\tUsage: -w numPoints X1 Y1 X2 Y2...\n");
}
} else {
printf("Unrecognized option [%s]\n",argv[i]);
return -1;
}
}
//Create thread to monitor robot movement
pthread_create(&thread, NULL, mapRobot, (void *)hands);
// robot is ready
#ifdef DEBUG
printf("All connections established, ready to go\n");
#endif
//Rotate the Servo
turret_SetServo(hands->t, T_ANGLE);
for(i=0; i<FILT_IR_SAMPLES; i++) {
filterIR(hands, filt, &t, &t);
#ifndef USE_IR_MODE
filterSonar(hands, filt, &t, &t);
#endif
}
//Iterate over all of the waypoints
for(i=0; i<numWaypts; i++) {
#ifdef DEBUG
printf("Waypoint %d (%f,%f).\n",i+1,waypoints[i].X,waypoints[i].Y);
#endif
Move(hands,filt,pids,waypoints[i].X,waypoints[i].Y);
#ifdef DEBUG
printf("\nArrived at waypoint %d (%f,%f).\n\n", i+1,waypoints[i].X,waypoints[i].Y);
#endif
}
// Shutdown and tidy up. Close all of the proxy handles
cleanup();
return 0;
}
/**
* @brief System starts here and delegates different actions.
* @param None
* @retval None
*/
void acc_thread(void const *argument){
st_system_state state = ST_INIT ;
err_type error = NONE;
float pitchRadians, rollRadians, pitchDegrees, rollDegrees;
/* global variable */
int init_time = 0;
filterState accPitchFilter;
filterState accRollFilter;
float filteredPitch=0.0;
float filteredRoll=0.0;
/* Main function */
while (1) {
/* The system can be in multiple states, this is just good coding practice */
switch ( state ){
/* Initialization state */
case ST_INIT :
/**************************************/
/************ initialization **********/
/**************************************/
//acc_init();
acc_ext_init();
initializeFilter(&accPitchFilter, 35);
initializeFilter(&accRollFilter, 35);
/**************************************/
state = ST_IDLE ;
break ;
/* Idle State */
case ST_IDLE :
/**************************************/
/**************** idle ****************/
/**************************************/
delay(init_time);
/**************************************/
state = ST_RUNNING ;
break ;
/* The system is running here the majority of the time */
case ST_RUNNING :
/**************************************/
/***************** run ****************/
/**************************************/
/* Accelerometer Interrupt. When high run the following: */
//if(interrupt_EXTI0 == 1){
osSignalWait(0x1,osWaitForever);
/* Reset flag */
interrupt_EXTI0 = 0;
/* Read accelerometer data X,Y,Z into */
float data[3];
//LIS3DSH_ReadACC(data);
//LSM9DS1_ReadACC(data);
LSM9DS1_ReadGYRO(data);
offsetAccData(data);
//printf("%f\t%f\t%f\n", data[0]/*x*/, data[1]/*y*/, data[2]/*z*/);
/* Calculate the pitch and roll in radians based on the following equation: */
/* Pitch = arctan(Ax1/sqrt((Ay1)^2+(Az1)^2 */
/* Roll = arctan(Ay1/sqrt((Ax1)^2+(Az1)^2 */
pitchRadians = calculatePitch(data);
rollRadians = calculateRoll(data);
/* Converting pitch and roll to degrees */
pitchDegrees = convertToDegrees(pitchRadians);
rollDegrees = convertToDegrees(rollRadians);
/* Filtering the pitch and roll angles in degrees using a moving average filter*/
filteredPitch = modify_filterState(&accPitchFilter, pitchDegrees);
filteredRoll = modify_filterState(&accRollFilter, rollDegrees);
/* Create message */
message_acc* acc_message = (message_acc*) osPoolAlloc(acc_mpool);
acc_message->pitch = filteredPitch;
acc_message->roll = filteredRoll;
osMessagePut(acc_mqueue, (uint32_t)acc_message, osWaitForever);
printf("%lf\t%f\t%lf\t%f\n", pitchDegrees, filteredPitch, rollDegrees, filteredRoll);
//}
/**************************************/
break ;
case ST_ERROR :
/**************************************/
/**************** error ***************/
/**************************************/
switch ( error ){
case NONE :
//You didn't set error type...
break ;
case ERR_ERROR_1 :
break ;
case ERR_ERROR_2 :
break ;
case ERR_ERROR_3 :
break ;
case ERR_ERROR_4 :
break ;
}
/**************************************/
break ;
}
//osDelay(250);
}
}
