AdvancedCoder
Posts: 3
Joined: Thu Dec 21, 2017 5:24 pm

wiringPi PWM problem?

Thu Dec 21, 2017 5:38 pm

Hello
I am working on a project. I want to build ball-on-beam system, and I have mechanical setup of the system. The problem is that I think encoder reading has a problem. Sometimes I send zero PWM, but encoder is counting. When I print counter, it shows that it is counting(with 0 PWM). I use wiringPi library. I checked motor and motor driver in Arduino, and it was working perfect. It has problem in Raspberry Pi. I think there should be a problem with library because I checked my code numerous times with online example codes.
There is another problem as well. No matter where is the ball on the beam, beam rotates in one direction. in1 and in2 are responsible for change in direction. Based on PWM sign, I change direction in the code, but I see that it rotates in one direction only.
BTW, I have a camera that I run it on my laptop and send its data to Raspberry PI using TCP/IP protocol by an ethernet cable. Since Raspberry PI is slow, I installed OpenCV on my laptop and send data(ball position) to Raspberry PI. Camera works perfect. My problem is the motor. I am in a point that I don't know what is the problem(I'm good at programming). I upload my code here (in controller part I calculate how much torque I need to send to the motor, and by using motor dynamic model I calculate required voltage to send to the motor. Finally I change this voltage to a PWM signal):
**************************************************************************************************************
#include <stdio.h>
#include <math.h>
#include <wiringPi.h>
#include <stdlib.h>
#include <stdint.h>
#include <signal.h>
#include <time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>

#define PI 3.14159265
#define etta 0.2
#define alpha 0.1
#define A_encoder 26
#define B_encoder 27
#define in1 0
#define in2 2
#define d1 6
#define d2 1
#define en 3
#define slew 4
#define inv 5

static volatile int counter = 0;
int AState;
int BState;
float M1_theta = 0.0;
float theta1 = 0.0;
float dtheta1 = 0.0;
float x = 0.0;
float dx = 0.0;

void SigHandler(){
printf("\nYou pressed Ctrl+C \n");
pwmWrite(d2, 1);
digitalWrite( in1, LOW);
digitalWrite( in2, LOW);
digitalWrite( d1, LOW);
digitalWrite( d2, LOW);
digitalWrite( en, LOW);
digitalWrite( slew, LOW);
digitalWrite( inv, LOW);
pinMode ( in1, INPUT);
pinMode ( in2, INPUT);
pinMode ( d1, INPUT);
pinMode ( d2, INPUT);
pinMode ( en, INPUT);
pinMode ( slew, INPUT);
pinMode ( inv, INPUT);
exit(1);
}

void encoder_reading(){
AState = digitalRead(A_encoder);
BState = digitalRead(B_encoder);
if(BState != AState){
counter += 1;
}
else{
counter -= 1;
}
M1_theta = counter*(360.0/3267)*(PI/180);
}

void Kzetax(float mat1[][4], float mat2[][1], float result[][1]){
int i, j, k;
for(i = 0; i < 1; i++){
for(j = 0; j < 1; j++){
for(k = 0; k < 4; k++){
result[j] += mat1[k]*mat2[k][j];
}
}
}
}

int main() {
signal( SIGINT, SigHandler);
float r = 2.3/200.0;
float mb = 6.28/1000.0;
float g = 9.81;
float Ip = 0.058181;
float Ib = (2.0/5.0)*mb*powf(r,2);
float l = 15.24/100;
float d = 9.3/100;
float Ke = 1.1459;
float Kt = 0.3107;
float R = 2.4;
float Tf = 0.0932;
float K[1][4] = {{-9.38, -21.44, -18.37, -7}};
if (wiringPiSetup() == -1){
exit(1);
}
int client_socket;
client_socket = socket(AF_INET, SOCK_STREAM, 0);
struct sockaddr_in server_address;
server_address.sin_family = AF_INET;
server_address.sin_port = htons(9002);
server_address.sin_addr.s_addr = inet_addr("192.168.1.5");
int connection_status = connect(client_socket, (struct sockaddr *) &server_address, sizeof(server_address));
if (connection_status < 0){
printf("There was an error connecting to the remote socket \n");
}
pinMode(A_encoder, INPUT);
pinMode(B_encoder, INPUT);
pullUpDnControl(A_encoder, PUD_DOWN);
pullUpDnControl(B_encoder, PUD_DOWN);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(d1, OUTPUT);
pinMode(d2, PWM_OUTPUT);
pinMode(en, OUTPUT);
pinMode(slew, OUTPUT);
pinMode(inv, OUTPUT);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(d1, LOW);
digitalWrite(d2, HIGH);
digitalWrite(en, HIGH);
digitalWrite(slew, LOW);
digitalWrite(inv, LOW);
pwmSetRange(4096);
pwmSetMode(PWM_MODE_BAL);
pwmSetClock(1);
wiringPiISR(A_encoder, INT_EDGE_RISING, &encoder_reading);
char buffer_x[10];
char buffer_y[10];
float prev_x = 0.0;
float prev_theta1 = 0.0;
float prev_dx = 0.0;
float prev_dtheta1 = 0.0;
float velocity_filter = 0.5;
int x_pixel = 0;
int iteration = 1;
clock_t prev_time = clock();
for(;;){
printf("interrupt counter is: %d \n",counter);
printf("interrupt M1_theta is: %f \n", M1_theta);
// Getting camera data from server
recv(client_socket, &buffer_x, sizeof(buffer_x), 0);
//printf("The server response is: %s\n", buffer_x);
sscanf(buffer_x, "%d", &x_pixel);
x = x_pixel*(29.5/320)*(1.0/100);
if(iteration == 1){
prev_x = x;
}

// Taking derivative from position states
clock_t curr_time = clock();
double elapsed_time = (double)(curr_time - prev_time)/CLOCKS_PER_SEC;
printf("elapsed_time is: %f \n", elapsed_time);
prev_time = curr_time;
dx = (x - prev_x)/elapsed_time;
dx = velocity_filter*dx + (1-velocity_filter)*prev_dx;
prev_dx = dx;
prev_x = x;
printf("dx is: %f \n", dx);
theta1 = (d/l)*M1_theta;
dtheta1 = (theta1 - prev_theta1)/elapsed_time;
dtheta1 = velocity_filter*dtheta1+(1-velocity_filter)*prev_dtheta1;
prev_dtheta1 = dtheta1;
prev_theta1 = theta1;
printf("dtheta1 is: %f \n", dtheta1);

// Controller part
float zeta1x = x;
printf("zeta1x: %f \n", zeta1x);
float zeta2x = dx;
printf("zeta2x: %f \n", zeta2x);
float zeta3x = (mb*powf(r,2)*(x*powf(dtheta1,2)-g*sin(theta1)))/(mb*powf(r,2)+Ib);
printf("zeta3x: %f \n", zeta3x);
float zeta4x = (powf(dtheta1,2)*dx*mb*powf(r,2))/(mb*powf(r,2)+Ib)-(dtheta1*g*mb*powf(r,2)*cos(theta1))/(mb*powf(r,2)+Ib)-(2*dtheta1*mb*powf(r,2)*x*(2*Ib*dtheta1*dx*mb*x+2*Ip*dtheta1*dx*mb*x+Ib*g*mb*x*cos(theta1)+Ip*g*mb*x*cos(theta1)))/((mb*powf(r,2)+Ib)*(Ib+Ip)*(mb*powf(x,2)+Ib+Ip));
printf("zeta4x: %f \n", zeta4x);
float zetax[4][1] = {{zeta1x},{zeta2x},{zeta3x},{zeta4x}};
float v1[1][1] = {{0}};
Kzetax(K, zetax, v1);
float Lf4hx = dtheta1*((dtheta1*g*mb*powf(r,2)*sin(theta1))/(mb*powf(r,2) + Ib) + (2*dtheta1*mb*powf(r,2)*x*(Ib*g*mb*x*sin(theta1) + Ip*g*mb*x*sin(theta1)))/((mb*powf(r,2) + Ib)*(Ib + Ip)*(mb*powf(x,2) + Ib + Ip))) - dx*((2*dtheta1*mb*powf(r,2)*(2*Ib*dtheta1*dx*mb*x + 2*Ip*dtheta1*dx*mb*x + Ib*g*mb*x*cos(theta1) + Ip*g*mb*x*cos(theta1)))/((mb*powf(r,2) + Ib)*(Ib + Ip)*(mb*powf(x,2) + Ib + Ip)) + (2*dtheta1*mb*powf(r,2)*x*(Ib*g*mb*cos(theta1) + Ip*g*mb*cos(theta1) + 2*Ib*dtheta1*dx*mb + 2*Ip*dtheta1*dx*mb))/((mb*powf(r,2) + Ib)*(Ib + Ip)*(mb*powf(x,2) + Ib + Ip)) - (4*dtheta1*powf(mb,2)*powf(r,2)*powf(x,2)*(2*Ib*dtheta1*dx*mb*x + 2*Ip*dtheta1*dx*mb*x + Ib*g*mb*x*cos(theta1) + Ip*g*mb*x*cos(theta1)))/((mb*powf(r,2) + Ib)*(Ib + Ip)*powf((mb*powf(x,2) + Ib + Ip),2))) + (((g*mb*powf(r,2)*cos(theta1))/(mb*powf(r,2) + Ib) - (2*dtheta1*dx*mb*powf(r,2))/(mb*powf(r,2) + Ib) + (2*mb*powf(r,2)*x*(2*Ib*dtheta1*dx*mb*x + 2*Ip*dtheta1*dx*mb*x + Ib*g*mb*x*cos(theta1) + Ip*g*mb*x*cos(theta1)))/((mb*powf(r,2) + Ib)*(Ib + Ip)*(mb*powf(x,2) + Ib + Ip)) + (2*dtheta1*mb*powf(r,2)*x*(2*Ib*dx*mb*x + 2*Ip*dx*mb*x))/((mb*powf(r,2) + Ib)*(Ib + Ip)*(mb*powf(x,2) + Ib + Ip)))*(2*Ib*dtheta1*dx*mb*x + 2*Ip*dtheta1*dx*mb*x + Ib*g*mb*x*cos(theta1) + Ip*g*mb*x*cos(theta1)))/((Ib + Ip)*(mb*powf(x,2) + Ib + Ip)) + (mb*powf(r,2)*((powf(dtheta1,2)*mb*powf(r,2))/(mb*powf(r,2) + Ib) - (2*dtheta1*mb*powf(r,2)*x*(2*Ib*dtheta1*mb*x + 2*Ip*dtheta1*mb*x))/((mb*powf(r,2) + Ib)*(Ib + Ip)*(mb*powf(x,2) + Ib + Ip)))*(x*powf(dtheta1,2) - g*sin(theta1)))/(mb*powf(r,2) + Ib);
printf("Lf4hx: %f \n", Lf4hx);
float Lgzeta4x = ((g*mb*powf(r,2)*cos(theta1))/(mb*powf(r,2) + Ib) - (2*dtheta1*dx*mb*powf(r,2))/(mb*powf(r,2) + Ib) + (2*mb*powf(r,2)*x*(2*Ib*dtheta1*dx*mb*x + 2*Ip*dtheta1*dx*mb*x + Ib*g*mb*x*cos(theta1) + Ip*g*mb*x*cos(theta1)))/((mb*powf(r,2) + Ib)*(Ib + Ip)*(mb*powf(x,2) + Ib + Ip)) + (2*dtheta1*mb*powf(r,2)*x*(2*Ib*dx*mb*x + 2*Ip*dx*mb*x))/((mb*powf(r,2) + Ib)*(Ib + Ip)*(mb*powf(x,2) + Ib + Ip)))/(mb*powf(x,2) + Ib + Ip);
printf("Lgzeta4x: %f \n", Lgzeta4x);
float tau1 = (v1[0][0]-Lf4hx) / Lgzeta4x;
printf("tau1: %f \n", tau1);
float M1_tau = (d/l)*tau1/(cos(theta1)*cos(M1_theta))+Tf;
printf("M1_tau: %f \n", M1_tau);
float voltage = ((R*M1_tau)/Kt) + powf(Kt,2.0)*dtheta1;
printf("voltage: %f \n", voltage);
float M1_PWM = 0.0;
if(voltage >= 0){
M1_PWM = (4096/12.2)*voltage;
if(M1_PWM > 4096){
M1_PWM = 4096;
}
}
else{
M1_PWM = (4096/12.2)*voltage;
if(M1_PWM < -4096){
M1_PWM = -4096;
}
}
printf("M1_PWM: %f \n",M1_PWM);
int M1_PWM_int = 0;
M1_PWM_int = (int)M1_PWM;
printf("M1_PWM_int: %d \n", M1_PWM_int);
if(M1_PWM_int > 0){
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
printf("direction backward \n");
}
if(M1_PWM_int < 0){
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
M1_PWM_int = -1*M1_PWM_int;
printf("direction forward \n");
}

if(M1_PWM_int == 0){
M1_PWM_int = 1;
}

// Sending command to motors
pwmWrite(d2, M1_PWM_int);
iteration = 2;
}
return 0;
}
*************************************************************************************************************************************

Any help highly appreciated.

Return to “Troubleshooting”