This update changes the motor control to regulate current on both sides of the motors. Two motors are connected, per channel, in parallel. The Motor Shield can only control two dc motors so the 4wd motors are wired together per side.
TimerInterupt
This Algorithm has a Timer Interrupt. Triggering a an function that reads motor pins A0 and A1 for the current and implements a feedback loop.
void timer_handler(int irq){
switch(irq){
case SIGALRM:
average_currentA=analogRead(currentPinA);
average_currentB=analogRead(currentPinB);
// average_currentA/=2;
// average_currentB/=2; //digitalWrite(13,(digitalRead(13)^0×01));
int diffa=abs(setspeedA)-average_currentA;
int diffb=abs(setspeedB)-average_currentB;
// printf("%i,%i %i,%i\n",average_currentA,average_currentB,diffa,diffb);
curspeedA+=diffa*0.1;
curspeedB+=diffb*0.1;
if(curspeedA<0)curspeedA=0;
if(curspeedB<0)curspeedB=0;
if(setspeedA>0)
digitalWrite(dirA,HIGH);
else digitalWrite(dirA,LOW);
if(setspeedB<0)
digitalWrite(dirB,HIGH);
else digitalWrite(dirB,LOW);
analogWrite(speedA,int(abs(curspeedA)));
analogWrite(speedB,int(abs(curspeedB)));
break;
};
}
Update to the rover.c file:
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <sys/types.h>
#include <time.h>
#include <core.h>
int dirA = 12;
int dirB = 13; // not used in this example
int speedA = 3;
int speedB = 11; // not used in this example
int currentPinA = A0;
int currentPinB = A1;
int average_currentA,average_currentB;
int setspeedA,setspeedB;
float curspeedA,curspeedB;
int listenfd = 0, connfd = 0;
int n;
struct sockaddr_in serv_addr;
char sendBuff[1025];
time_t ticks;
unsigned int counter;
void timer_handler(int irq);
//////////////////////////////////////////////////////////////////////////////////////
#define REFRESH_TIME_S 0
#define REFRESH_TIME_US 5000
void time_initialization(long s,long us)
{
struct itimerval interrupt_time;
interrupt_time.it_value.tv_sec = s;
interrupt_time.it_value.tv_usec =us;
interrupt_time.it_interval.tv_sec = s;
interrupt_time.it_interval.tv_usec = us;
setitimer(ITIMER_REAL, &interrupt_time, NULL);
signal(SIGALRM, timer_handler);
printf("set interrupt time: %lds , %ldus\n", s, us);
}
/**
* readline() - read an entire line from a file descriptor until a newline.
* functions returns the number of characters read but not including the
* null character.
**/
int readline(int fd, char *str, int maxlen)
{
int n; /* no. of chars */
int readcount; /* no. characters read */
char c;
for (n = 1; n < maxlen; n++) {
/* read 1 character at a time */
readcount = read(fd, &c, 1); /* store result in readcount */
if (readcount == 1) /* 1 char read? */
{
*str = c; /* copy character to buffer */
str++; /* increment buffer index */
if (c == '\n') /* is it a newline character? */
break; /* then exit for loop */
}
//else if (readcount == 0) /* no character read? */
// {
// if (n == 1) /* no character read? */
// return (0); /* then return 0 */
// else
// break; /* else simply exit loop */
// }
else
return (-1); /* error in read() */
}
*str=0; /* null-terminate the buffer */
return (n); /* return number of characters read */
} /* readline() */
void setup()
{
counter=0;
pinMode (dirA, OUTPUT);
pinMode (dirB, OUTPUT);
pinMode (speedA, OUTPUT);
pinMode (speedB, OUTPUT);
digitalWrite(speedA,LOW);
digitalWrite(speedB,LOW);
listenfd = socket(AF_INET, SOCK_STREAM, 0);
memset(&serv_addr, '0', sizeof(serv_addr));
memset(sendBuff, '0', sizeof(sendBuff));
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);
serv_addr.sin_port = htons(5000);
bind(listenfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr));
listen(listenfd, 10);
connfd = accept(listenfd, (struct sockaddr*)NULL, NULL);
printf("connected\n");
curspeedA=0;curspeedB=0;
setspeedA=0;setspeedB=0;
average_currentA=0;
average_currentB=0;
time_initialization(REFRESH_TIME_S,REFRESH_TIME_US);
}
///////////////////////////////////////////////////////////////////////////////////////////////////
void timer_handler(int irq){
switch(irq){
case SIGALRM:
average_currentA=analogRead(currentPinA);
average_currentB=analogRead(currentPinB);
// average_currentA/=2;
// average_currentB/=2; //digitalWrite(13,(digitalRead(13)^0×01));
int diffa=abs(setspeedA)-average_currentA;
int diffb=abs(setspeedB)-average_currentB;
// printf("%i,%i %i,%i\n",average_currentA,average_currentB,diffa,diffb);
curspeedA+=diffa*0.1;
curspeedB+=diffb*0.1;
if(curspeedA<0)curspeedA=0;
if(curspeedB<0)curspeedB=0;
if(setspeedA>0)
digitalWrite(dirA,HIGH);
else digitalWrite(dirA,LOW);
if(setspeedB<0)
digitalWrite(dirB,HIGH);
else digitalWrite(dirB,LOW);
analogWrite(speedA,int(abs(curspeedA)));
analogWrite(speedB,int(abs(curspeedB)));
break;
};
}
/////////////////////////////////////////////////////////////////////////////////////////////////
void loop()
{
n=readline(connfd,sendBuff,8);
// n = read(connfd, sendBuff, strlen(sendBuff) );
printf("%d:",n);
if(n>0)
{
printf("%s",sendBuff);
if(sendBuff[0]=='B'){
digitalWrite(speedA,LOW);
digitalWrite(speedB,LOW);
}
else if(sendBuff[0]=='C'){
char *p=sendBuff;p++;
int k=strtol(p,&p,10);
if(k<0){
digitalWrite(dirA,HIGH);
digitalWrite(dirB,HIGH);
}
else {
digitalWrite(dirA,LOW);
digitalWrite(dirB,LOW);
}
setspeedA=(k)*10;
setspeedB=(k)*10;
// analogWrite(speedA,abs(k)*10);
//analogWrite(speedB,abs(k)*10);
}
else if(sendBuff[0]=='A')
{
char *p=sendBuff;
p++;
int j=strtol(p,&p,10);
printf("%d\n",j);
setspeedA=abs(j);
setspeedB=abs(j);
//analogWrite(speedB,abs(j));
// analogWrite (speedA,abs(j));
// delay(100);
}
else if(sendBuff[0]=='E'){
// snprintf(sendBuff, sizeof(sendBuff), "%i,%i\n",analogRead(currentPinA),analogRead(currentPinB));
// write(connfd, sendBuff, strlen(sendBuff));
//printf("%s",sendBuff);
}
}
else { //reconnect
setspeedA=0;setspeedB=0;
digitalWrite(speedA,LOW);
digitalWrite(speedB,LOW);
connfd = accept(listenfd, (struct sockaddr*)NULL, NULL);
printf("connected\n");
}
// close(connfd);
// sleep(1);
}
No comments:
Post a Comment