Show amigo.c syntax highlighted
/*
*
* Copyright (c) 2003 The Regents of the University of California. All
* rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* - Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* - Neither the name of the University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
* OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#include <math.h>
#include <stdio.h>
#include <unistd.h>
#include <pthread.h>
#include "uart.h"
#include "amigo.h"
//#define AMIGO_PRINT_CONSOLE 1
pthread_t psndr, rcv;
pthread_mutex_t amigoio, amigolistenlock, amigoexit;
pthread_cond_t amigolistencond, amigoexitcond;
COMM_HANDLE hCOM;
unsigned char sndmsg[SND_MSG_LEN], rcvmsg[RCV_MSG_LEN];
int motor_guard, stop_listen=0, amigo_close;
void* do_receive(void *arg);
short calc_chksum(unsigned char *ptr);
#ifdef AMIGO_PRINT_CONSOLE
void print()
{
unsigned char j;
printf("send: ");
for(j=0;j<sndmsg[2]+3;j++)
printf("%x ",sndmsg[j]);
printf("\n");
}
#endif
//::::::::::::::
//amigo_func.c
//::::::::::::::
int amigoMoveRel(float x1,float y1,float x2,float y2,short flag,float vel,float time)
{
float xnew,ynew;
xnew = x1 + x2;
ynew = y1 + y2;
amigoMoveAbs(x1,y1,xnew,ynew,flag,vel,time);
return 1;
}
int amigoRotateAbs(float deg, short flag, float rvel, float time)
{
signed short turn_angle;
signed short turn_vel;
if(flag)
turn_vel=(short)(abs(deg-amigoChkOrientation())/(time*.9));
else
turn_vel=(short)rvel;
// turn_angle = (short)deg;
turn_angle = (short)deg-1;
while(amigoChkMotorBusy());
amigoDoTurn(turn_angle, turn_vel);
return 1;
}
int amigoRotateRel(float deg, short flag, float rvel, float time)
{
signed short turn_vel;
// amigoCmdSetRV(MAXRV);
// amigoCmdSetRA(MAXRA);
if(flag)
turn_vel=(short)(abs(deg)/(time*.9));
else
turn_vel=(short)rvel;
amigoCmdSetRV(turn_vel);
amigoCmdSetRA(MAXRA);
amigoCmdDHead(deg);
while(amigoChkMotorBusy());
return 1;
}
int amigoHead(float x1,float y1,float x2, float y2)
{
float theta;
signed short turn_angle;
theta = atan2((y2-y1),(x2-x1))*180/PI;
turn_angle = (short)(theta);
//printf( "turn angle: %d\n", turn_angle );
/*debug*//*
if(turn_angle > 0)
//turn_angle -= 2;
;
else
//turn_angle += 2;
turn_angle = 360 + turn_angle;
*/
// printf("theta %f turn_angle %d",theta,turn_angle);
while(amigoChkMotorBusy());
// if(turn_angle!=0)
amigoDoTurn(turn_angle, MAXRV);
return 1;
}
int amigoMoveAbs(float x1,float y1,float x2,float y2,short flag,float vel,float time)
{
float distance,dx,dy;
signed short r,xtep,ytep;
xtep=(x2-x1)*1000/.5083;
ytep=(y2-y1)*1000/.5083;
dx=x2-x1;
dy=y2-y1;
distance=sqrt((dx*dx)+(dy*dy));
if(flag)
vel=amigoDoTimeToVel(distance,time);
vel=vel*1000;
while(amigoChkMotorBusy());
amigoHead(x1,y1,x2,y2);
while(amigoChkMotorBusy());
// moving to a point and avoid collision to obstacle
r = 0;
do{
r = amigoDoMove(vel,xtep,ytep);
amigoCmdStop();
while(amigoChkMotorBusy());
// if there is collision
if(r == -1)
{
//unsigned short xa, ya;
float xf, yf;
receive_bytes(hCOM, rcvmsg);
xf = XPOS;
yf = YPOS;
//printf("\nfirst XPOS: %d, YPOS: %d", XPOS, YPOS);
// rotate 90 degree
amigoRotateRel(90.0, 0, 0.4, 0);
// go straight
while(amigoChkMotorBusy());
amigoCmdVel(vel);
// if there is something in front, then turn 180 degree
// future work
while(1)
{
receive_bytes(hCOM, rcvmsg);
dx=XPOS-xf;
dy=YPOS-yf;
distance=sqrt((dx*dx)+(dy*dy));
// 500 is just an empirical value
//if( distance > 500 )
if( distance > 0.3 )
break;
}
// stop the robot
amigoCmdStop();
while(amigoChkMotorBusy());
// turn head to the destenation
//x1 = amigoChkX();
//y1 = amigoChkY();
//xtep=(x2-x1)*1000/.5083;
//ytep=(y2-y1)*1000/.5083;
xtep=(x2-XPOS)*1000/.5083;
ytep=(y2-YPOS)*1000/.5083;
//printf("\nXPOS: %d, YPOS: %d", XPOS, YPOS);
//xf = ((float)(XPOS-16384))*0.5083*0.001;
//yf = ((float)(YPOS-16384))*0.5083*0.001;
//printf("\nxf: %f, yf: %f, x2: %f, y2: %f", xf, yf, x2, y2);
amigoHead(XPOS,YPOS,x2,y2);
while(amigoChkMotorBusy());
}
}while(r == -1);
//printf("\nXPOS: %d, YPOS: %d", XPOS, YPOS);
return 1;
}
int amigoDoMove(float vel,signed short xtep,signed short ytep){
unsigned short xlastpos,ylastpos;
unsigned char flag1,flag2,flag_first;
char* sonarinfo;
flag1=0;flag2=0;
// printf("vel %f xtep %d ytep %d",vel,xtep,ytep);
// printf("\nSetV..");
amigoCmdSetV(SETV);
// printf("\nSetA..");
amigoCmdSetA(SETA);
// printf("\nVel..");
amigoCmdVel(vel);
// printf("Vel%f",vel);
flag_first=1;
xlastpos = ylastpos = 0;
while(1)
{
if(flag_first)
{
xlastpos = amigoChkX();
ylastpos = amigoChkY();
flag_first=0;
}
if(xtep==0)
flag1=1;
if(ytep==0)
flag2=1;
// printf(" xpos %d ypos %d xorigin %d yorigin %d ",amigo_info.xpos,amigo_info.ypos,xlastpos,ylastpos);
if(abs(amigoChkX()-xlastpos)>abs(xtep))
flag1=1;
if(abs(amigoChkY()-ylastpos)>abs(ytep))
flag2=1;
if(flag1 && flag2)
// break;
return 0;
// perform sonar detection
sonarinfo = amigoChkSonar();
if( SONARVALUE0 < 400 )
{
// the front sonar
if( (SONARNUM0 > 0) && (SONARNUM0 < 5) )
return -1;
}
else if( SONARVALUE1 < 400 )
{
if( (SONARNUM1 > 0) && (SONARNUM1 < 5) )
return -1;
}
}
}
void amigoDoTurn(short beta, short turn_speed)
{
short orientation;
// printf("beta %d",beta);
// printf("\nSetRV..");
// amigoCmdSetRV(MAXRV);
amigoCmdSetRV(turn_speed);
// printf("beta %d",beta);
// printf("\nSetRA..");
amigoCmdSetRA(MAXRA);
orientation=(short)(amigoChkOrientation()*.001534);
#ifdef AMIGO_PRINT_CONSOLE
printf("orientation %d",orientation);
#endif
if(beta!=orientation)
amigoCmdHead(beta);
// printf("motor_stat %x \n",motor_stat);
//wait until done, then return
while(amigoChkMotorBusy());
// printf("I am in amigoDoTurn\n");
}
float amigoDoTimeToVel(float distance,float time)
{
float vel;
float time_new;
time_new=0.8 * time;
vel=distance/time_new;
return vel;
}
//::::::::::::::
//amigo_cmd.c
//::::::::::::::
void* dumbSender(void *arg)
{
// printf("dumb_snder started..\n, amigo_close: %d",amigo_close);
while(1)
{
amigoCmdPulse();
sleep(2);
// printf("PulseSend.. ");
pthread_mutex_lock(&amigoexit);
if(amigo_close)
break;
pthread_mutex_unlock(&amigoexit);
}
pthread_mutex_unlock(&amigoexit);
// printf("I pass this line 2..\n");
// while(!dumb_sndr_stop)
// dumb_sndr_stop = 1;
// printf("pass the line 2 !!");
pthread_cond_signal(&amigoexitcond);
pthread_exit(NULL);
}
void amigoCmdHead(short r)
{
int orien, count=0;
orien = amigo_info.orientation;
amigoCmdTypeB(HEAD, r);
amigoSetMotorBusy();
usleep(500000); // 500 ms for response time
receive_bytes(hCOM, rcvmsg);
while(count<3)
{
if(orien == amigo_info.orientation)
count++;
usleep(100000);
orien = amigo_info.orientation;
// printf("I am Head\n");
receive_bytes(hCOM, rcvmsg); // 120202
}
}
void amigoCmdDHead(short r)
{
int orien, count=0;
orien = amigo_info.orientation;
amigoCmdTypeB(DHEAD, r);
amigoSetMotorBusy();
usleep(500000); // 500 ms for response time
receive_bytes(hCOM, rcvmsg);
while(count<3)
{
if(orien == amigo_info.orientation)
count++;
usleep(100000);
orien = amigo_info.orientation;
receive_bytes(hCOM, rcvmsg); // 120202
}
}
void amigoCmdSetRA(short a)
{
amigoCmdTypeB(SETRA, a);
}
void amigoCmdSetRV(short s)
{
amigoCmdTypeB(SETRV, s);
}
void amigoCmdVel(short v)
{
amigoCmdTypeB(VEL, v);
amigoSetMotorBusy();
}
void amigoCmdSetV(short v)
{
amigoCmdTypeB(SETV, v);
}
void amigoCmdSetA(short a)
{
amigoCmdTypeB(SETA, a);
}
void amigoCmdStop(void)
{
stop_listen = 1;
amigoCmdTypeA(STOP);
amigoSetMotorStop();
// amigoCmdPulse(); //120202
// usleep(500000); //120202
// amigoCmdDHead(0);
amigoCmdTypeB(DHEAD, 0);
// usleep(500000);
// printf("after 500 ms\n");
// while(amigoChkMotorBusy());
// stop_listen = 0;
}
void amigoCmdSetO(void)
{
amigoCmdTypeA(SET0);
}
void amigoCmdClose(void)
{
// while(!amigo_close)
// amigo_close = 1;
// printf("I pass this line\n");
// XXX there will be no return
// while(!dumb_sndr_stop);
// printf("pass the line 1 !!");
pthread_mutex_lock(&amigoexit);
amigo_close = 1;
pthread_cond_wait(&amigoexitcond, &amigoexit);
pthread_mutex_unlock(&amigoexit);
amigoCmdTypeA(CLOSE);
//clean up
pthread_mutex_destroy(&amigoio);
pthread_mutex_destroy(&amigolistenlock);
pthread_mutex_destroy(&amigoexit);
pthread_cond_destroy(&amigolistencond);
pthread_cond_destroy(&amigoexitcond);
close_comm_port(hCOM);
memset( sndmsg, 0, SND_MSG_LEN );
memset( rcvmsg, 0, RCV_MSG_LEN );
}
void amigoCmdOpen(void)
{
amigoCmdTypeA(OPEN);
}
void amigoCmdPulse(void)
{
// if(pthread_mutex_trylock(&amigoio) == 0) //120202
{
amigoCmdTypeA(PULSE);
// pthread_mutex_unlock(&amigoio); //120202
}
// else
// printf("mutex error in CmdPulse\n");
}
void amigoCmdEnable(short e)
{
amigoCmdTypeB(ENABLE, e);
}
void amigoCmdTypeA(unsigned short cmd)
{
pthread_mutex_lock(&amigoio);
sndmsg[0] = 0xfa;
sndmsg[1] = 0xfb;
sndmsg[2] = 0x03;
sndmsg[3] = cmd;
sndmsg[4] = (char)(calc_chksum(&sndmsg[2]) >> 8);
sndmsg[5] = (char)(calc_chksum(&sndmsg[2]));
// pthread_mutex_lock(&amigoio);
write_buffer(hCOM, &sndmsg[0], 6);
#ifdef AMIGO_PRINT_CONSOLE
print();
#endif
// if(!amigo_close)
receive_bytes(hCOM, rcvmsg);
pthread_mutex_unlock(&amigoio);
}
void amigoCmdTypeB(unsigned short cmd, signed short p)
{
pthread_mutex_lock(&amigoio);
if( p > 0 )
sndmsg[4] = 0x3b;
else
{
sndmsg[4] = 0x1b;
p = abs(p);
}
if(cmd == SETA)
sndmsg[4] = 0x1b;
sndmsg[0] = 0xfa;
sndmsg[1] = 0xfb;
sndmsg[2] = 0x06;
sndmsg[3] = cmd;
sndmsg[5] = p%256;
sndmsg[6] = p/256;
sndmsg[7] = (char)(calc_chksum(&sndmsg[2]) >> 8);
sndmsg[8] = (char)(calc_chksum(&sndmsg[2]));
// pthread_mutex_lock(&amigoio);
#ifdef AMIGO_PRINT_CONSOLE
print();
#endif
write_buffer(hCOM, &sndmsg[0], 9);
receive_bytes(hCOM, rcvmsg);
pthread_mutex_unlock(&amigoio);
}
void amigoCmdConn()
{
char i;
amigo_close = 0;
hCOM = open_comm_port("/dev/tts/0",9600,8,0,0);
// hCOM = open_comm_port("/dev/ttySA0",9600,8,0,0);
if(hCOM == -1)
{
printf("COM Port Open Failed!\n");
return;
}
for(i=0;i<3;i++)
{
#ifdef AMIGO_PRINT_CONSOLE
printf("SYN%d Send..\n", i);
#endif
amigoCmdTypeA(i);
if(i<2)
{
char c;
for(c=0; c<6; c++)
{
if(sndmsg[c] != rcvmsg[c])
break;
}
if(c!=6)
i=0;
}
}
amigoCmdOpen();
#ifdef AMIGO_PRINT_CONSOLE
printf("Command OPEN\n");
print();
#endif
amigoCmdEnable(1);
#ifdef AMIGO_PRINT_CONSOLE
printf("Command Enable\n");
print();
#endif
// amigoReceiverInit(); // then start receiver //120202
if( pthread_create( &psndr, NULL, dumbSender, NULL) )
printf("Error to create pthread.\n"); //120202
//amigoMoveAbs(0,0,0.5,0.5,0,0.8,0);
//amigoCmdStop();
/* while(pthread_mutex_trylock(&amigolistenlock)) //120202
{
printf("is lock");
pthread_cond_signal(&amigolistencond);
}
stop_listen = 0;
printf("stop1:%d ",stop_listen);
if(pthread_mutex_unlock(&amigolistenlock)) printf("Error 2");
if(pthread_cond_signal(&amigolistencond)) printf("Error 3");
*/
// amigoCmdDHead(270);
//amigoMoveRel(0.5,0.5,-0.5,-0.5,0,0.8,0);
// amigoHead(1, 1, -1, -1 );
//amigoCmdDHead(180);
//printf("Next is Last");
//amigoCmdHead(0);
}
short calc_chksum(unsigned char *ptr)
/* ptr is array of bytes, first is data count*/
{
short n;
short c = 0;
n = *(ptr++);
n -= 2;
while(n > 1)
{
c += (*(ptr)<<8) | *(ptr+1);
c = c & 0xffff;
n -= 2;
ptr += 2;
}
if(n > 0)
c = c ^ (int)*(ptr++);
return c;
}
//::::::::::::::
//amigo_chk.c
//::::::::::::::
int amigoReceiverInit()
{
return pthread_create( &rcv, NULL, do_receive, NULL);
}
char *amigoChkSonar()
{
receive_bytes(hCOM, rcvmsg);
return amigo_info.sonar;
}
unsigned short amigoChkX()
{
// usleep(100000); // 100ms 120202
receive_bytes(hCOM, rcvmsg); // 120202
return amigo_info.xpos;
}
unsigned short amigoChkY()
{
// usleep(100000); // 100ms 120202
receive_bytes(hCOM, rcvmsg); // 120202
return amigo_info.ypos;
}
signed short amigoChkOrientation()
{
// usleep(100000); // 100ms 120202
receive_bytes(hCOM, rcvmsg); // 120202
return amigo_info.orientation;
}
unsigned short amigoChkMotorBusy()
{
// usleep(100000); // 100ms 120202
receive_bytes(hCOM, rcvmsg); // 120202
if( amigo_info.motor_stat == 0x32 ) // motors stop
return 0;
else if( amigo_info.motor_stat == 0x33 ) // motors moving
return 1;
else
return 2;
}
void amigoSetMotorStop()
{
amigo_info.motor_stat=0x32;
}
void amigoSetMotorBusy()
{
amigo_info.motor_stat=0x33;
motor_guard = 1;
}
void* do_receive(void *arg)
{
// while(!amigo_close)
while(1)
{
if(pthread_mutex_lock(&amigolistenlock)) printf("Error 5");
while(stop_listen)
{
printf("stop2:%d ",stop_listen);
if(pthread_cond_wait(&amigolistencond, &amigolistenlock)) printf("Error 6");
}
if(pthread_mutex_unlock(&amigolistenlock)) printf("Error 7");
if(pthread_mutex_lock(&amigoio)) printf("Error 8");
receive_bytes(hCOM, rcvmsg);
if(pthread_mutex_unlock(&amigoio)) printf("Error 9");
}
// printf("hohoho.. this also passed\n");
pthread_exit(NULL);
}
unsigned char receive_bytes(COMM_HANDLE hCOM, unsigned char *rcvmsg)
{
unsigned char c, l;
Start:
while(1)
{
while(!bytes_available(hCOM));
get_byte(hCOM, &rcvmsg[0]); // header byte 0xFA
if(rcvmsg[0] != 0xFA)
continue;
while(!bytes_available(hCOM));
get_byte(hCOM, &rcvmsg[1]);
if(rcvmsg[1] == 0xFB)
break;
}
while(!bytes_available(hCOM));
get_byte(hCOM, &rcvmsg[2]); // number of following byte
l = rcvmsg[2];
if( l > RCV_MSG_LEN )
goto Start;
for(c=0; c<l; c++)
{
while(!bytes_available(hCOM));
get_byte(hCOM, &rcvmsg[3+c]); // receive
}
#ifdef AMIGO_PRINT_CONSOLE
printf("receive: ");
for(c=0;c<l+3;c++)
printf("%x ",rcvmsg[c]);
printf("\n\n");
#endif
if( (motor_guard == 1) && (rcvmsg[3]==0x32) )
;
else
{
amigo_info.motor_stat = rcvmsg[3];
motor_guard = 0;
}
amigo_info.xpos = (16383+((rcvmsg[5]&0x7F) << 8) + rcvmsg[4]) & 0x7fff;
amigo_info.ypos = (16383+((rcvmsg[7]&0x7F) << 8) + rcvmsg[6]) & 0x7fff;
amigo_info.orientation = (rcvmsg[9] << 8) + rcvmsg[8];
amigo_info.lvel = (rcvmsg[11] << 8) + rcvmsg[10];
amigo_info.rvel = (rcvmsg[13] << 8) + rcvmsg[12];
amigo_info.battery = rcvmsg[14];
for(c=0; c<6; c++)
amigo_info.sonar[c] = rcvmsg[23+c];
return l+3;
}
See more files for this project here