int8_t RotaryEncoder::readNav() {
static int8_t inc = 0;
inc += readEncoder();
if ( inc/4 ) {
int8_t retVal = inc/4;
inc = 0;
return retVal;
}
return 0;
}
float getPositionMine(int group){
float val=0;
if(group<numPidMotors){
if(pidGroups[group].config.Enabled || vel[group].enabled)
val = readEncoder(group);
}else{
val = getHeaterTempreture(group);
}
return val;
}
void production_control_timer(xTimerHandle pxTimer){ // Executed every millisecond
set_debug(DEBUG8, true);
set_led(LED3, true);
// Get Set-Point from supervisor through IOI though FIFO
set_debug(DEBUG7, true);
plantParams.set_point = getSetPoint();
plantParams.theta_des = plantParams.set_point*pi/180;
//xil_printf("%d\n\r", plantParams.set_point); // uncomment if you want to print set-point at each control cycle to reconfig UART
// Read the two encoder sensors
set_debug(DEBUG7, false);
plantParams.encoder_theta = -readEncoder(SS_ENCODER_S) % 4096;
plantParams.thetaR = plantParams.encoder_theta*Kenc;
set_debug(DEBUG7, true);
plantParams.encoder_alpha= 4096+(-readEncoder(SS_ENCODER_P) % 4096);
plantParams.alphaR = plantParams.encoder_alpha*Kenc-pi;
// Calculate the control algorithm if pendulum is upright
set_debug(DEBUG7, false);
if((plantParams.alphaR >= 0 ? plantParams.alphaR:-plantParams.alphaR) < (20.*pi/180)){
// Comment out whatever controller you don't want to use
plantParams.u = calculateKalmanControlSignal(&plantParams);
//plantParams.u = calculateStateFeedbackControlSignal(&plantParams);
}
else {
plantParams.u = 0;
}
// Write voltage to the servo
set_debug(DEBUG7, true);
writeDAC(plantParams.u);
set_debug(DEBUG7, false);
++plantParams.cycle_count;
++cycleCounter;
set_led(LED3, false);
set_debug(DEBUG8, false);
}
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;
}
/* Run a command. Commands are defined in commands.h */
int runCommand() {
int i = 0;
char *p = argv1;
char *str;
int pid_args[4];
arg1 = atoi(argv1);
arg2 = atoi(argv2);
switch(cmd) {
case GET_TICKS:
Serial.println(ticks);
break;
case GET_BAUDRATE:
Serial.println(BAUDRATE);
break;
case ANALOG_READ:
Serial.println(analogRead(arg1));
break;
case DIGITAL_READ:
Serial.println(digitalRead(arg1));
break;
case ANALOG_WRITE:
analogWrite(arg1, arg2);
Serial.println("OK");
break;
case DIGITAL_WRITE:
if (arg2 == 0) digitalWrite(arg1, LOW);
else if (arg2 == 1) digitalWrite(arg1, HIGH);
Serial.println("OK");
break;
case PIN_MODE:
if (arg2 == 0) pinMode(arg1, INPUT);
else if (arg2 == 1) pinMode(arg1, OUTPUT);
Serial.println("OK");
break;
case PING:
Serial.println(Ping(arg1));
break;
#ifdef USE_SERVOS
case SERVO_WRITE:
servos[arg1].write(arg2);
Serial.println("OK");
break;
case SERVO_READ:
Serial.println(servos[arg1].read());
break;
#endif
#ifdef USE_BASE
case READ_ENCODERS:
Serial.print(readEncoder(LEFT));
Serial.print(" ");
Serial.println(readEncoder(RIGHT));
break;
case RESET_ENCODERS:
resetEncoders();
Serial.println("OK");
break;
case READ_MOTORS:
Serial.print(leftPID.output);
Serial.print(" ");
Serial.println(rightPID.output);
break;
case MOTOR_SPEEDS:
/* Reset the auto stop timer */
lastMotorCommand = millis();
if (arg1 == 0 && arg2 == 0) {
setMotorSpeeds(0, 0);
moving = 0;
resetPID();
}
else moving = 1;
leftPID.TargetTicksPerFrame = arg1;
rightPID.TargetTicksPerFrame = arg2;
Serial.println("OK");
break;
case UPDATE_PID:
while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str);
i++;
}
Kp = pid_args[0];
Kd = pid_args[1];
Ki = pid_args[2];
Ko = pid_args[3];
Serial.println("OK");
break;
#endif
default:
Serial.println("Invalid Command");
break;
}
}
/*
void enc_poll_nonblocking_tick(const uint8_t bogus __attribute__((unused)))
Description: Reads encoders values.
Returns: void
Encoders connections:
0 -> Port I2C1
1 -> Port SPI
2 -> Port I2C3
*/
void enc_poll_nonblocking_tick(const uint8_t encoderNumber)
{
static uint8_t i2cPort1Updated = 0;
static uint8_t i2cPort3Updated = 0;
int result;
switch(enc_poll_state)
{
case EPS_DONE:
// printf("encoder\n");
result = setEncoderRegister(1, AS5048B_ANGLLSB_REG, SPI_TIMEOUT);
result = readEncoder(1, SPI_TIMEOUT); // (spiPort, encoderNumber, timeout)
if (isBusyI2CPort(handPorts.encoder[0]) && isBusyI2CPort(handPorts.encoder[2]))
{
// printf("neither 0 nor 2\n");
enc_poll_state = EPS_I2C;
i2cPort1Updated = 0;
i2cPort3Updated = 0;
}
else if (!isBusyI2CPort(handPorts.encoder[0]) && !isBusyI2CPort(handPorts.encoder[2]))
{
// printf("0 and 2, EPS_DONE\n");
result = setEncoderRegister(0, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
result = readEncoder(0, I2C_TIMEOUT); // (i2cPort, encoderNumber, timeout)
result = setEncoderRegister(2, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
result = readEncoder(2, I2C_TIMEOUT); // (i2cPort, encoderNumber, timeout)
enc_poll_state = EPS_DONE;
}
else if (!isBusyI2CPort(handPorts.encoder[2]))
{
// printf("0\n");
result = setEncoderRegister(0, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
result = readEncoder(0, I2C_TIMEOUT); // (i2cPort, encoderNumber, timeout)
enc_poll_state = EPS_DONE;
}
else if (!isBusyI2CPort(handPorts.encoder[0]))
{
// printf("2\n");
result = setEncoderRegister(2, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
result = readEncoder(2, I2C_TIMEOUT); // (i2cPort, encoderNumber, timeout)
enc_poll_state = EPS_DONE;
}
break;
case EPS_I2C:
if (!isBusyI2CPort(handPorts.encoder[0]) && !isBusyI2CPort(handPorts.encoder[2]))
{
// printf("0 and 2, EPS_I2C\n");
result = setEncoderRegister(0, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
result = readEncoder(0, I2C_TIMEOUT); // (i2cPort, encoderNumber, timeout)
result = setEncoderRegister(2, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
result = readEncoder(2, I2C_TIMEOUT); // (i2cPort, encoderNumber, timeout)
enc_poll_state = EPS_DONE;
}
else if (!isBusyI2CPort(handPorts.encoder[2]) && i2cPort1Updated == 0)
{
// printf("0\n");
result = setEncoderRegister(0, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
result = readEncoder(0, I2C_TIMEOUT); // (i2cPort, encoderNumber, timeout)
i2cPort1Updated = 1;
if (i2cPort3Updated == 1)
enc_poll_state = EPS_DONE;
}
else if (!isBusyI2CPort(handPorts.encoder[0]) && i2cPort3Updated == 0)
{
// printf("2\n");
result = setEncoderRegister(2, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
result = readEncoder(2, I2C_TIMEOUT); // (i2cPort, encoderNumber, timeout)
i2cPort3Updated = 1;
if (i2cPort1Updated == 1)
enc_poll_state = EPS_DONE;
}
break;
default:
enc_poll_state = EPS_DONE; // shouldn't get here
break;
result = result + 1 - 1;
// printf("%d\n", result); // rmelo19, to correct, change to error checking.
}
}
void TC5_Handler()
{
//unedited/old:
/* // TcCount16* TC = (TcCount16*) TC3; // get timer struct
if (TC5->COUNT16.INTFLAG.bit.OVF == 1) { // A overflow caused the interrupt
interrupted = 1;
TC5->COUNT16.INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag
// irq_ovf_count++; // for debug leds
}
// if (TC->INTFLAG.bit.MC0 == 1) { // A compare to cc0 caused the interrupt
// digitalWrite(pin_mc0_led, LOW); // for debug leds
// TC->INTFLAG.bit.MC0 = 1; // writing a one clears the flag ovf flag
// } */
///new
// TcCount16* TC = (TcCount16*) TC3; // get timer struct
if (TC5->COUNT16.INTFLAG.bit.OVF == 1) { // A overflow caused the interrupt
a = readEncoder();
y = lookup_angle(a);
if ((y - y_1) < -180.0) {
wrap_count += 1;
}
else if ((y - y_1) > 180.0) {
wrap_count -= 1;
}
//y_1 = y; pushed lower
yw = (y + (360.0 * wrap_count));
switch (mode) {
case 'x':
e = (r - yw);
ITerm += (pKi * e);
if (ITerm > 150) ITerm = 150;
else if (ITerm < -150) ITerm = -150;
u = ((pKp * e) + ITerm - (pKd * (yw - yw_1))); //ARDUINO library style
//u = u+lookup_force(a)-20;
// u = u_1 + cA*e + cB*e_1 + cC*e_2; //ppt linked in octave script
// u = 20*e;//
break;
case 'v':
e = (r - ((yw - yw_1) * 500));//416.66667)); degrees per Tc to rpm
ITerm += (vKi * e);
if (ITerm > 200) ITerm = 200;
else if (ITerm < -200) ITerm = -200;
u = ((vKp * e) + ITerm - (vKd * (yw - yw_1)));//+ lookup_force(a)-20; //ARDUINO library style
break;
case 't':
u = 1.0 * r ;//+ 1.7*(lookup_force(a)-20);
break;
default:
u = 0;
break;
}
//
// if (u > 0) {
// PA = 1.8;
// }
// else {
// PA = -1.8;
// }
//
// y += PA;
if (u > 0) {
y+=PA;
}
else {
y -=PA;
}
if (u > uMAX) { //saturation limits max current command
u = uMAX;
}
else if (u < -uMAX) {
u = -uMAX;
}
U = abs(u); //+lookup_force((((a-4213)%16384)+16384)%16384)-6); ///p);//+i);
if (abs(e) < 0.1) {
digitalWrite(pulse, HIGH);
SerialUSB.println(r);
}
else {
digitalWrite(pulse, LOW);
}
output(-y, U); //-y
e_3 = e_2;
e_2 = e_1;
e_1 = e;
u_3 = u_2;
u_2 = u_1;
u_1 = u;
yw_1 = yw;
y_1 = y;
TC5->COUNT16.INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag
}
}
