navigation of hedgehog robot

1 post / 0 new
Author
Message
#1
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi my name is Rudy, I am from India and I am currently doing my Post grad dip in electrical engg at New Zealand. I havent been taught about ATMEGA128 micro controller bck in India and I havent been taught abt the programming here as well but i had been given assignments to be completed which i have no clue how to do it. I dont even knw a signle line in the programming. I some have managed to submit the first assignment but I am struck with my 2nd cuz I have no clue where to add what codes. The 2nd assignment is basically a modification of the first one. The objective of the first assignment is to make a HEDGEHOG ROBOT TO MOVE TO A DESIRED TARGET.

The objective of the second assignment is to NAVIGATE THE HEDGEHOG ROBOT BY WRITING A PROGRAM FO RTHE ROBOT WHICH RECEIVES THE COMMANDS IN THE SERIAL PORT.

I have typed the coding for the first assignment which my friend helped me to do. 

#include <iom128.h>
#include <intrinsics.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
/*******************************************************************************
* DEFINES
*******************************************************************************/
#define PI 3.141592653589793238462643 // from CLIB math.h
#define RIGHT_SPEED 1023
#define LEFT_SPEED 1023
#define BUF_MAX 1000
#define USE_SERIAL_INTERRUPTS 1
/*******************************************************************************
* STRUCTURES
*******************************************************************************/
struct Position
{
 float x; //  position coordinates in metres
 float y;
 float h; // heading angle in radians, anti-clockwise from the x-axis
};
struct Target
{

    float x;
    float y;
    float hdg;
};
/*******************************************************************************
* FUNCTION PROTOTYPES
*******************************************************************************/
void Setup(void); // ATmega128 initialisation for this program
void CalcPosition(void); // calculate robot position from wheel pulse counts
void OutputString(char* str); // put string in serial port 0 transmit buffer
void MotorFunction();//robot state function//
void PIDfunction(int start,float eNew);//calculates PWM to guide robot
/*******************************************************************************
* GLOBAL VARIABLES
*******************************************************************************/
volatile unsigned int leftPulseCount, rightPulseCount; // wheel pulse counts
struct Position pos; // current position from CalcPosition
struct Target tgt; // target position
 volatile int robotState;//
enum robotState{START, TURN, PID, STOP};
 float distance = 0;
 volatile int PIDstate = 0;
#if USE_SERIAL_INTERRUPTS == 1
// serial port 0 transmit buffer
char buffer[BUF_MAX]; // buffer (queue) data
volatile unsigned int head, tail, count; // buffer (queue) indexes
#endif
/*******************************************************************************
* MAIN FUNCTION
*******************************************************************************/
void main (void)
{
 static char str[40]; // serial output string

 Setup(); // initialise ATmega128

 sprintf(str, "SPEED,%d,%d\r\n", LEFT_SPEED, RIGHT_SPEED); // display the wheel pwm settings
 OutputString(str);
 while (1) // loop forever
{
CalcPosition(); // calculate position from wheel counters
 MotorFunction();//switch state which also calls PID function,initialised with START state
 }
}
/*******************************************************************************
* OTHER FUNCTIONS
*******************************************************************************/
void Setup(void) // ATmega128 setup
{
 // timer1 setup for pwm: enable pwm outputs, mode 7 prescale=256

   TCCR1B = 0x0C; // 00001100
   TCCR1A = 0xA3; // 10100011
   OCR1A = 0; // motors off
   OCR1B = 0;

   DDRB_Bit5 =1; // enable motor outputs
   DDRB_Bit6 =1;

    DDRA_Bit6=1;// enable motor direction outputs, motors forward
    DDRA_Bit7=1;
    PORTA_Bit6=0;
    PORTA_Bit7=0;

    DDRC_Bit3=1; // enable the wheel pulse generator electronics
    PORTC_Bit3=1;

    UCSR0B_Bit4=1;// serial output: enable receive and transmit
    UCSR0B_Bit3=1;

    UCSR0C_Bit2=1;// 8 data bits, no parity, 1 stop
    UCSR0C_Bit1=1;
    UCSR0C_Bit5=0;
    UCSR0C_Bit4=0;
    UCSR0C_Bit3=0;

    // set the baud rate
      UBRR0L=12;

 // do NOT enable the transmit interrupt here

 __delay_cycles(4000000); // 500ms delay give the electronic hardware time to settle

      EIMSK = 0x03;// enable external interrupts for the wheel pulses (INT0, INT1)
      EICRA = 0x0F; // 00001111;

 // set initial robot location at origin
 pos.x = 0.;
 pos.y = 0.;
 pos.h = 0.;
 #if USE_SERIAL_INTERRUPTS == 1
 // initialise serial output buffer
 head = 0;
 tail = 0;
 count = 0;
 #endif
 __enable_interrupt(); // enable interrupts last
 // display started on serial port
 OutputString("\r\nstart\r\n");

//*****************ctc*timer 3*calculation***************//

    TCCR3A  = 0x00;//chA, clear at OCR, ctc mode with OCR3A top value
    TCCR3B  = 0x0C;//CTC mode
        //calculation with 256 prescaler: (8e6/256)/3125 = 10hz = 100ms.

    OCR3A  = 3124; //set OCR at 3124 ;

    ETIMSK  = 0x10;//timer 3 OCRA match interrupt enable.

   //set TARGET//
    tgt.x = -1;//set the hedgehog to travel 1 metre straight then stop.
    tgt.y = 0;
    tgt.hdg = 0;
    UCSR0B_RXCIE0 = 1;  //enables serial recieve interrupt
   __enable_interrupt();
}

//Position Calculation//
 void CalcPosition(void)
{
 int leftCount, rightCount; // number of wheel pulses
 static char str[100]; // serial output string
 // get the pulse counts
 __disable_interrupt();
 leftCount = leftPulseCount; // copy the pulse counts
 rightCount = rightPulseCount;
 leftPulseCount = 0; //initialise variables
 rightPulseCount = 0;

 __enable_interrupt();

 // if there were any pulses, calculate the new position
 if (leftCount || rightCount)
 {
 // do the position calculation
   float dx,dy,dtheta;//creates variables for change in axis//

 if(leftCount==rightCount)  //hedgehog travelled in straight line formula//

 {
   dx = .007114*rightCount*cos(pos.h);
   dy = .007114*leftCount*sin(pos.h);
 }
  else
   {
   //if robot doesnot travel in stright line formula//
     dtheta=(rightCount-leftCount)*(.06686);
     dx=.0532*(rightCount+leftCount)/(rightCount-leftCount)*(sin(pos.h+dtheta)-sin(pos.h));
     dy=.0532*(rightCount+leftCount)/(rightCount-leftCount)*(cos(pos.h)-cos(pos.h+dtheta));
   }
 // display the new position
                       pos.x=pos.x+dx;//stores new x-axis value
                       pos.y=pos.y+dy;//stores new y-axis value
                       pos.h=pos.h+dtheta;//stores new heading value, Radians//
       // display the new position on PC//
 sprintf(str, "POS,%7.3f,%7.3f,%7.1f,%3d,%3d\r\n", pos.x, pos.y,
 pos.h * 180. / PI, leftCount, rightCount);
 OutputString(str);
 }
}
#if USE_SERIAL_INTERRUPTS == 1
// transmit serial string USING INTERRUPTS
void OutputString(char* str)
{
 int length = strlen(str);
 UCSR0B_UDRIE0 = 0; // disable serial port 0 UDRE interrupt
 // check for too many chars
 if (count + length >= BUF_MAX)
 {
 UCSR0B_UDRIE0 = 1; // enable serial port 0 UDRE interrupt
 return;
 }
 // write the characters into the buffer
 for (int n = 0; n < length; n++)
 {
 buffer[tail] = str[n];
 tail++;
 if (tail >= BUF_MAX)
 {
 tail = 0;
 }
 }
 count += length;
 UCSR0B_UDRIE0 = 1; // enable serial port 0 UDRE interrupt
}
#else
// transmit serial string NOT USING INTERRUPTS
void OutputString(char* str)
{
 int length = strlen(str);
 // for each character in the string
 for (int n = 0; n < length; n++)
 {
 // wait while the serial port is busy
 while (!UCSR0A_UDRE0);
 // transmit the character
 UDR0 = str[n];
 }
}
#endif

//*******************P*I*D************************//

void PIDfunction(int start, float eNew) // calculate the robot current position//
{

    float F;
    float pL, pR;
    static float eSum;//when the robot turns eSum=0//
    static float ePrev;//IF statement for initial zero 

    //error calculation//

    if (start)
    {//clears integral sum obtained while turning//
      eSum = 0;
      ePrev = eNew;
    }
    else
    {

    eSum = (eSum + (ePrev + eNew));
    }

    if (eSum > 20)//reset if sum gets too big
    {
        eSum = 20;//
    }
    if (eSum < -20)//reset if sum gets too big in negative plane
    {
        eSum = -20;//
    }
    //F variable for PWMf motors//
    F = 2000*eNew + 100*(eSum) + 750*(eNew - ePrev);

    if (F >1023)//limits motor speeds from getting too large//
      F = 1023;
     if (F < -1023)
      F = -1023;
   //store ePrev for next calculation//
    ePrev = eNew;
    //PWM for each wheel//
    if(F >0)
    {
    pL = 1023 ;
    pR = 1023 - F;
    }
    else if(F < 0)
    {
      pR=1023;
      pL= 1023+ F;
    }
    else
    {
      pL=1023;
      pR =1023;
    }
    //operate the motors with new values of pL and pR//
    OCR1A = (int)pL;//set motor speeds based on PID output, turns motors on//
    OCR1B = (int)pR;
}

//**************MOTOR*STATE*LOGIC***************//

void MotorFunction()
{
  static int start;
  float eNew;
 //calculate target heading from current position//
       tgt.hdg = atan2((tgt.y - pos.y), (tgt.x - pos.x));  

//error signal of PID controller//
        //removes 2Pi error when crossing 2Pi mark//
        eNew = pos.h - tgt.hdg;
          if(eNew > PI)
            eNew = eNew - 2*PI;

          if(eNew < -PI)
            eNew = eNew + 2*PI;

 // calculate the remaing distance between target and current position //
        distance = sqrt( ((tgt.x - pos.x)*(tgt.x - pos.x)) + ((tgt.y - pos.y)*(tgt.y - pos.y)) );

        switch (robotState)
    {
    case START:
        if (distance <= 0.05)// 0.05 metres, at target
        {
            robotState = STOP;//enters STOP case in next cycle
            OutputString("MOTOR,STOP1\r\n");
        }
        else if (eNew > 0.4 || eNew < -0.4)//heading, radians, large error
        {
            robotState = TURN;
            OutputString("MOTOR,TURN1\r\n");
        }
        else
        {
            robotState = PID; //when small heading error
            OutputString("MOTOR,PID1\r\n");
            start = 1;//send this to clear integral sum
        }
        break;

    case TURN:  //large error, sharp turn with one wheel STOPPED
            //integral off
        if (distance <= 0.05)//millimetres, from target
        {
            robotState = STOP;
            OutputString("MOTOR,STOP2\r\n");
        }
       if (eNew > 0.4)//turn RIGHT
        {
            OCR1A = 1023;//LEFT motor ON
            OCR1B = 0;//RIGHT motor off
        }
        else if (eNew < -0.4)//turn LEFT
        {
            OCR1A = 0;//LEFT motor off.
            OCR1B = 1023;//RIGHT motor ON
        }

        else
        {
            robotState = PID;//small error go in PID function
            OutputString("MOTOR,PID2\r\n");
            start = 1;
        }
        break;

    case PID: //when small heading error, use PID to guide robot to remaining distance.
        if (distance <= 0.05)// from target
        {
            robotState = STOP;
            OutputString("MOTOR,STOP3\r\n");
        }
        else if (eNew > 0.4 || eNew < -0.4)
        {
            robotState = TURN;//if  error increased, large error
            OutputString("MOTOR,TURN3\r\n");
        }
        else
        {
            if (PIDstate == 1)//call PID calculation function once 100ms has elapsed.
            {
                PIDstate = 0;//reset PID timer;
                PIDfunction(start, eNew);//majority of time is spent here., PID will guide heading.
                start = 0;
            }
        }
        break;
        case STOP:
            OCR1A = 0;//LEFT, seize motors, turns motors off.
            OCR1B = 0;//RIGHT,
            //continue position calculation, due to rolling after stopping motors
        break;
    }

}

/*******************************************************************************
* INTERRUPT FUNCTIONS
*******************************************************************************/
#pragma vector = INT0_vect
__interrupt void LeftCounterISR(void) // left wheel pulse counter
{
 leftPulseCount++;
}
#pragma vector = INT1_vect
__interrupt void RightCounterISR(void) // right wheel pulse counter
{
 rightPulseCount++;
}
#if USE_SERIAL_INTERRUPTS == 1
#pragma vector = USART0_UDRE_vect
__interrupt void Serial0UDREmptyISR(void) // serial DRE (transmit) interrupt
{
 if (count > 0) // if there are more characters
 {
 UDR0 = buffer[head]; // transmit the next character
 // adjust the buffer variables
 head++;
 if (head > BUF_MAX)
 {
 head = 0;
 }
 count--;
 }
 if (count == 0) // if there are no more characters
 {
 UCSR0B_UDRIE0 = 0; // then disable serial port 0 UDRE interrupt
 }
}
#endif
//timer3 interrupt function-----------------------------
#pragma vector = TIMER3_COMPA_vect
__interrupt void PIDstateISR(void)//PID switch
{
    PIDstate = 1;//allows main funtion to call PID function.
}

*************************************************************************************************************

I have to modify the above program to get the second assignment. I have now typed the sample code for assignment 2 which I have alrdy been provided with.

struct Position
{
    float x; // metres
    float y;
    float h; // radians, anti-clockwise from x-axis
};

struct Target
{
    int cmd; // type of command (GO or TURN or STOP)
    float x;
    float y;
    float h;
    int s; // speed = maximum pwm = 0-1023
};

 

// global variables for serial receive interrupt function
volatile char gotCmd; // got command flag
volatile char command[40]; // received command string

// serial receive interrupt function
#pragma vector = USART0_RXC_vect
__interrupt void SerialReceiveISR(void)
{
    char ch;
    static char buffer[40]; // receiving command
    static int index = 0; // number of command chars received
    ch = UDR0;
    // only store ASCII characters
    if (ch >= ' ' && ch <= '~' && index < 39)
    {
        buffer

= ch;
        index++;
    }
    if (ch == '\r') // end of command
    {
        buffer
= 0; // null character code
        // make sure the previous command has been used
        if (!gotCmd)
        {
            strcpy(command, buffer);
            gotCmd = 1;
        }
        index = 0;
    }    
}

 

// sample code to extract values from the command strings
// GO_CMD can be an enum (preferred) or a #define value
int n;
float f1, f2;
if (strncmp(command, "GO", 2) == 0)
{
    // get x and y values
    n = sscanf(command + 2, "%f,%f", &f1, &f2);
    if (n == 2) // got both numbers
    {
        // set command type for the Navigation state machine
        target->cmd = GO_CMD;
        target->x = f1; // change the target coordinates
        target->y = f2;
    }
}

*************************************************************************

I need to navigation commands like "SPEED S", "GO X,Y", "TURN D", "STOP". CAN someone please help me with this programming and help me get the code for the second assignment. It will be of great help. I need to submit it bfore 16/05/2016. very short period of time I am sorry I just came to kno about this site. Thank you          

 

rudy johnnson