int main(void)
{
/*
if(wiringPiSetupGpio()==-1)
return 1;
int gpio1 = 27;
softPwmCreate(gpio1, 0, 50);
usleep(100000);
while (1)
{
softPwmWrite(gpio1, 10);
usleep(100000);
}
return 0;
*/
//if(wiringPiSetupGpio()==-1)
//return 1;
const int desiredDistFromWall = 110 ;
wiringPiSetup();
pinMode(gpio_steering,PWM_OUTPUT);
pinMode(gpio_lidarPan,PWM_OUTPUT);
pwmSetMode(PWM_MODE_MS);
pwmSetClock(375);
pwmSetRange (1024);
softPwmCreate(gpio1, 0, 200);
softPwmCreate(gpio2, 0, 200);
//softPwmCreate(gpio_pixyPan , 0, 200);
//softPwmCreate(gpio_pixyTilt, 0, 200);
softPwmCreate(gpio_lidarTilt,0, 200);
//initializeServos(gpio_steering,gpio_pixyPan,gpio_pixyTilt,gpio_lidarPan,gpio_lidarTilt);
int lidFd = wiringPiI2CSetup(0x62);
int fd = wiringPiI2CSetup(0x30);
int newFd = changeEncoderAddress(fd, 0x10);
//printf("fd = %d\r\n", newFd);
double result = readEncoder(newFd);
int fd2 = wiringPiI2CSetup(0x30);
int newFd2 = changeEncoderAddress(fd2, 0x11);
//printf("fd2 = %d\r\n", newFd2);
double result2 = readEncoder(newFd2);
double dt = 100000;
double integral = 0;
double previousError = 0;
double Kp = 0.01;
double Ki = 0.0008;
double Kd = 1;
double error = 0;
double derivative = 0;
double output = 0;
int lidVal = 0;
int lidarPanServoInd [3] = {20 , 50, 70};
int pixyPanServoInd [3] = {0 , 5, 10};
// setServo(gpio_pixyTilt,22);
// usleep(30000);
// setServo(gpio_pixyTilt,0);
setServo(gpio_lidarTilt,20);
usleep(30000);
setServo(gpio_lidarTilt,0);
setHardServo(gpio_lidarPan,70);
double avg = desiredDistFromWall;
int errFromWall=0;
for (int i = 0; i < 200; i++)
{
lidVal = readLidar (lidFd);
if (lidVal >0 )
{ // steering based on lidVal
avg = (lidVal+avg*3)/(3.0+1) ;
}
errFromWall = avg-desiredDistFromWall;
if (errFromWall>5)
setHardServo(gpio_steering,60);
else if (errFromWall<-5)
setHardServo(gpio_steering,40);
else
setHardServo(gpio_steering,50);
printf ("\n lidVal: %d",lidVal) ;
printf ("\n Avg distance: %f",avg) ;
//setServo(gpio_steering,i);
result = readEncoder(newFd);
result2 = readEncoder(newFd2);
error = result + result2 ;
integral += error ;
derivative = (error - previousError)/dt ;
output = Kp*error + Ki*integral + Kd * derivative;
printf ("\n LEFT ENCODER: %f -- RIGHT encoder: %f -- Error: %f \n", result,result2,error);
moveForward (17 , output) ;
usleep(dt);
}
setHardServo(gpio_steering,0);
setHardServo(gpio_lidarPan,0);
return 0;
}
/******************************************************************
* Main-function
*/
int main(void) {
/**************************************
* Set the clock to run at 80 MHz
*/
SysCtlClockSet(SYSCTL_SYSDIV_2_5|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|SYSCTL_OSC_MAIN);
/**************************************
* Enable the floating-point-unit
*/
FPUEnable();
// We also want Lazystacking, so enable that too
FPULazyStackingEnable();
// We also want to put numbers close to zero to zero
FPUFlushToZeroModeSet(FPU_FLUSH_TO_ZERO_EN);
/**************************************
* Init variables
*/
uint16_t mapData[MAP_DATA_SIZE];
uint8_t stepDir = 0;
/**************************************
* Init peripherals used
*/
bluetooth_init();
init_stepper(); // Init the GPIOs used for the stepper and loading LED
InitI2C1(); // Init the communication with the lidar-unit through I2C
InitPWM();
setupTimers();
/**************************************
* State 2
*/
// Disable the timers that are used
disableTimer(TIMER1_BASE);
disableTimer(TIMER2_BASE);
// Enable all interrupts
IntMasterEnable();
/**************************************
* State 3
*/
// Indicate we should start with a scan regardless of what other things we have already got
// from UART-interrupt
// This means setting the appropriate bit in the status vector
stat_vec |= TAKE_MEAS;
/**************************************
* State 4
*/
// Contains main-loop where decisions should be made
for ( ; ; ) {
/**********************************
* Decision tree
*/
// Highest priority case first
// Check both interrupts at each iteration in the loop
if ( int_vec & UART_INT ) {
// Reset the indication
int_vec &= ~UART_INT;
// Remove drive-stop flag to enable movement
stat_vec &= ~DRIVE_STOP;
// Init data array
uint8_t dataArr[MAX_UART_MSG_SIZE];
// Collect the message
if ( readUARTMessage(dataArr, MAX_UART_MSG_SIZE) < SUCCESS ) {
// If we have recieved more data than fits in the vector we should simply
// go in here again and grab data
int_vec |= UART_INT;
}
// We have gathered a message
// and now need to determine what the message is
parseMsg(dataArr, MAX_UART_MSG_SIZE);
}
// Checking drive (movement) interrupt
if ( int_vec & TIMER2_INT ) {
int_vec &= ~TIMER2_INT;
// Disable TIMER2
disableTimer(TIMER2_BASE);
// Set drive-stop in status vector
stat_vec |= DRIVE_STOP;
}
// Checking measure interrupt
if ( int_vec & TIMER1_INT ) {
int_vec &= ~TIMER1_INT;
// Disable TIMER1
disableTimer(TIMER1_BASE);
// Take reading from LIDAR
mapData[stepCount++] = readLidar();
SysCtlDelay(2000);
// Take step
// Note: We need to take double meas at randvillkor (100) !!!!!
if ( stepCount > 0 && stepCount < 100 ) {
stepDir = 1;
}
else if ( stepCount >= 100 && stepCount < 200) {
stepDir = 0;
}
else {
stepDir = 1;
stepCount = 0;
// Reset busy-flag
stat_vec &= ~TAKE_MEAS;
}
step = takeStep(step, stepDir);
// Request reading from LIDAR
reqLidarMeas();
if ( stat_vec & TAKE_MEAS ) {
// Restart TIMER1
enableTimer(TIMER1_BASE, (SYSCLOCK / MEASUREMENT_DELAY));
}
else {
sendUARTDataVector(mapData, MAP_DATA_SIZE);
stat_vec &= ~BUSY;
}
}
// Check the drive_stop flag, which always should be set unless we should move
if ( stat_vec & DRIVE_STOP ) {
// Stop all movement
SetPWMLevel(0,0);
halt();
// MAKE SURE all drive-flags are not set
stat_vec &= ~(DRIVE_F | DRIVE_L | DRIVE_R | DRIVE_LL | BUSY);
}
// Should we drive?
else if ( stat_vec & DRIVE ) {
// Remove drive flag
stat_vec &= ~DRIVE;
// Increase PWM
increase_PWM(0,MAX_FORWARD_SPEED,0,MAX_FORWARD_SPEED);
if ( stat_vec & DRIVE_F ) {
enableTimer(TIMER2_BASE, DRIVE_FORWARD_TIME);
}
else if ( stat_vec & DRIVE_LL ) {
enableTimer(TIMER2_BASE, DRIVE_TURN_180_TIME);
}
else {
enableTimer(TIMER2_BASE, DRIVE_TURN_TIME);
}
}
if ( !(stat_vec & BUSY) ) {
// Tasks
switch ( stat_vec ) {
case ((uint8_t)DRIVE_F) :
// Call drive function
go_forward();
// Set the drive flag & BUSY
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)DRIVE_L) :
// Call drive-left function
go_left();
// Set the drive flag
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)DRIVE_R) :
// Call drive-right function
go_right();
// Set the drive flag
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)DRIVE_LL) :
// Call turn 180-degrees function
go_back();
// Set the drive flag
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)TAKE_MEAS) :
// Request reading from LIDAR
reqLidarMeas();
// Start TIMER1
enableTimer(TIMER1_BASE, (SYSCLOCK / MEASUREMENT_DELAY)); // if sysclock = 1 s, 1/120 = 8.3 ms
// We are busy
stat_vec |= BUSY;
break;
default:
break;
}
}
}
}
