rendered paste body/*
step.c
Program to verify new algorithm for linear acceleration.
Author: Pramod Ranade <pramod.ranade@spjsystems.com>
*/
#include <stdio.h>
#include <io.h>
#include <stdlib.h>
#define SCAN_TIME_IN_MICRO_SEC 1
#define COUNT_FOR_COMPARISON (1000000000UL / SCAN_TIME_IN_MICRO_SEC)
#define SPEED_LIMIT 9500
#define ACC_LIMIT 900000
#define START_SPEED 1500
#define DES_POS 117
#define AVAILABLE_TIME_IN_MILI_SEC 23
int OutputPinStatus = 1 ;
int g_nRisingEdgeCount = 0 ;
FILE *fptr ;
#define MakeOutputHigh() OutputPinStatus=1
#define MakeOutputLow() {PrintDebugInfo(uiDoneSteps, uiCurTimeInMicroSec, uiCurSpeedX1K);OutputPinStatus=0;}
void PrintDebugInfo (unsigned int uiDoneSteps, unsigned int uiCurTimeInMicroSec, unsigned int uiCurSpeedX1K)
{
fprintf(fptr,"%u\t%u\t%u\n", uiDoneSteps, uiCurTimeInMicroSec, uiCurSpeedX1K) ;
}
int FpgaRunMotor (unsigned int uiDesPos, unsigned int uiStartSpeedX1K, unsigned int uiDeltaSpeedX1K, unsigned int uiTimeForAccInMicroSec, unsigned int uiTimeToStartDeccInMicroSec, unsigned int uiPeakSpeed)
{
/*
This function performs the actions which must be done by FPGA.
So, finally this should be converted to VHDL or Verilog code for the FPGA.
*/
unsigned int uiDoneSteps ;
unsigned int uiCurSpeedX1K ;
unsigned int uiSpeedTimeProduct ;
unsigned int uiCurTimeInMicroSec ;
MakeOutputHigh() ;
fprintf(fptr,"Inside FpgaRunMotor:\n") ;
fprintf(fptr,"uiDesPos = %u\n", uiDesPos) ;
fprintf(fptr,"uiStartSpeedX1K = %u\n", uiStartSpeedX1K) ;
fprintf(fptr,"uiDeltaSpeedX1K = %u\n", uiDeltaSpeedX1K) ;
fprintf(fptr,"uiTimeForAccInMicroSec = %u\n", uiTimeForAccInMicroSec) ;
fprintf(fptr,"uiTimeToStartDeccInMicroSec = %u\n", uiTimeToStartDeccInMicroSec) ;
uiDoneSteps = 0 ;
uiCurSpeedX1K = uiStartSpeedX1K ;
uiDoneSteps ++ ;
uiSpeedTimeProduct = uiCurSpeedX1K ;
uiCurTimeInMicroSec = 0 ;
MakeOutputLow() ; // first pulse output
while(uiDoneSteps < uiDesPos)
{
// execute this loop after every N micro-seconds, where N = SCAN_TIME_IN_MICRO_SEC
uiCurTimeInMicroSec += SCAN_TIME_IN_MICRO_SEC ;
uiSpeedTimeProduct += uiCurSpeedX1K ;
if (uiCurTimeInMicroSec >= uiTimeToStartDeccInMicroSec)
{
// it means deceleration is going on
if (uiCurSpeedX1K > uiDeltaSpeedX1K)
{
uiCurSpeedX1K -= uiDeltaSpeedX1K ;
}
else
{
uiSpeedTimeProduct = COUNT_FOR_COMPARISON ;
printf("\t\tgadbad!\n") ;
}
}
else if (uiCurTimeInMicroSec < uiTimeForAccInMicroSec)
{
// it means acceleration is going on
uiCurSpeedX1K += uiDeltaSpeedX1K ;
}
else
{
// it means steady speed (plateau)
uiCurSpeedX1K = uiPeakSpeed ;
}
if (uiSpeedTimeProduct >= COUNT_FOR_COMPARISON)
{
uiDoneSteps ++ ;
MakeOutputLow() ; // next pulse output
printf("falling edge at step %d!\n", uiDoneSteps) ;
uiSpeedTimeProduct -= COUNT_FOR_COMPARISON ;
}
else if (uiSpeedTimeProduct >= (COUNT_FOR_COMPARISON / 2))
{
if (!OutputPinStatus)
{
printf("g_nRisingEdgeCount = %d\n", g_nRisingEdgeCount) ;
}
MakeOutputHigh() ;
g_nRisingEdgeCount ++ ;
}
}
MakeOutputHigh() ;
return 0 ;
}
int RunMotor (unsigned int uiStartSpeed, unsigned int uiAvailableTimeInMicroSec, int nDesPos, unsigned int uiSpeedLimit, unsigned int uiAccLimit)
{
/*
This function performs the calculations which must be done by microprocessor.
Finally, it calls another function FpgaRunMotor()
i.e. it passes some parameters to FPGA and tells it to start motion.
*/
unsigned int uiAverageSpeed ;
unsigned int uiTimeForAccInMicroSec ;
unsigned int uiTimeToStartDeccInMicroSec, uiRequiredAcc ;
unsigned int uiPeakSpeed, uiDeltaSpeedX1K ;
uiRequiredAcc = 0 ;
if (uiAvailableTimeInMicroSec)
{
uiAverageSpeed = abs(nDesPos) * 1000000UL / uiAvailableTimeInMicroSec ; // steps/sec
if (uiAverageSpeed <= uiStartSpeed)
{
// no acceleration/deccelration required
uiStartSpeed = uiAverageSpeed ;
}
else
{
uiRequiredAcc = 1 ;
}
}
else
{
// no acceleration/deccelration required
}
if (uiRequiredAcc)
{
if (uiAverageSpeed <= ((uiSpeedLimit + uiStartSpeed) / 2))
{
// tri-angular profile
uiTimeForAccInMicroSec = uiAvailableTimeInMicroSec / 2 ;
uiPeakSpeed = ((uiAverageSpeed - uiStartSpeed) * 2) + uiStartSpeed ;
uiRequiredAcc = (((uiPeakSpeed - uiStartSpeed) * 40000UL) / (uiAvailableTimeInMicroSec)) * 50 ;
// please do not try to reduce above stmnt
// becuase it results in overflow of intermediate result
}
else
{
// trapezoidal profile
/*
number of steps covered during acc and deacc = (Ta * (uiSpeedLimit + uiStartSpeed))
number of steps covered during steady speed = nDesPos - (Ta * (uiSpeedLimit + uiStartSpeed))
Ts = steady time can be computed from:
uiSpeedLimit = (nDesPos - (Ta * (uiSpeedLimit + uiStartSpeed))) / Ts ;
i.e.
Ts = (nDesPos - (Ta * (uiSpeedLimit + uiStartSpeed))) / uiSpeedLimit ;
(Ts + 2*Ta) * 1000000 = uiAvailableTimeInMicroSec ;
Here, Ts and Ta are in seconds.
(((nDesPos - (Ta * (uiSpeedLimit + uiStartSpeed))) / uiSpeedLimit) + (2*Ta)) * 1000000 = uiAvailableTimeInMicroSec ;
((nDesPos - (Ta * (uiSpeedLimit + uiStartSpeed))) / uiSpeedLimit) + (2*Ta) = uiAvailableTimeInMicroSec / 1000000 ;
multiply boths sides by uiSpeedLimit
(nDesPos - (Ta * (uiSpeedLimit + uiStartSpeed))) + (2*Ta*uiSpeedLimit) = uiAvailableTimeInMicroSec*uiSpeedLimit / 1000000 ;
nDesPos - Ta*uiSpeedLimit - Ta*uiStartSpeed + 2*Ta*uiSpeedLimit = uiAvailableTimeInMicroSec*uiSpeedLimit / 1000000 ;
nDesPos - Ta*(uiStartSpeed - uiSpeedLimit) = uiAvailableTimeInMicroSec*uiSpeedLimit / 1000000 ;
nDesPos - (uiAvailableTimeInMicroSec*uiSpeedLimit / 1000000) = Ta*(uiStartSpeed - uiSpeedLimit) ;
Ta = (nDesPos - (uiAvailableTimeInMicroSec*uiSpeedLimit / 1000000)) / (uiStartSpeed - uiSpeedLimit) ;
TaInUSec = (nDesPos*1000000 - uiAvailableTimeInMicroSec*uiSpeedLimit) / (uiStartSpeed - uiSpeedLimit) ;
TaInUSec = (uiAvailableTimeInMicroSec*uiSpeedLimit - nDesPos*1000000) / (uiSpeedLimit - uiStartSpeed) ;
*/
uiPeakSpeed = uiSpeedLimit ;
uiTimeForAccInMicroSec = (uiAvailableTimeInMicroSec*uiSpeedLimit - nDesPos*1000000) / (uiSpeedLimit - uiStartSpeed) ;
uiRequiredAcc = (((uiSpeedLimit - uiStartSpeed) * 40000U) / uiTimeForAccInMicroSec) * 25 ;
// please do not try to reduce above stmnt
// becuase it results in overflow of intermediate result
if (uiRequiredAcc > uiAccLimit)
{
// handle this error condition
// TBD
}
}
uiTimeToStartDeccInMicroSec = uiAvailableTimeInMicroSec - uiTimeForAccInMicroSec ;
uiDeltaSpeedX1K = SCAN_TIME_IN_MICRO_SEC * uiRequiredAcc / 1000 ;
if (!uiDeltaSpeedX1K)
{
uiDeltaSpeedX1K = 1 ;
}
// do something to set/clear direction pin here
FpgaRunMotor(abs(nDesPos), uiStartSpeed * 1000, uiDeltaSpeedX1K, uiTimeForAccInMicroSec, uiTimeToStartDeccInMicroSec, uiPeakSpeed * 1000) ;
}
else
{
// run steady at uiStartSpeed for N steps and then stop
// do something to set/clear direction pin here
FpgaRunMotor(abs(nDesPos), uiStartSpeed * 1000, 0, 0, uiAvailableTimeInMicroSec, uiStartSpeed * 1000) ;
}
fprintf(fptr,"Required acceleration = %u Steps/sec/sec\n", uiRequiredAcc) ;
return uiRequiredAcc ;
}
int main (int argc, char* argv[])
{
unsigned int uiStartSpeed = START_SPEED ;
unsigned int uiAvailableTimeInMiliSec = AVAILABLE_TIME_IN_MILI_SEC ;
int nDesPos = DES_POS ;
unsigned int uiSpeedLimit = SPEED_LIMIT ;
unsigned int uiAccLimit = ACC_LIMIT ;
int k ;
if ((argc < 2) || (argc > 7))
{
printf("Syntax: step outputfilename [AvailableTime] [DesPos] [StrtSpeed] [SpeedLimit] [AccLimit]\n") ;
return 1 ;
}
fptr = fopen(argv[1], "wt") ;
if (fptr == NULL)
{
printf("Could not open file %s\n", argv[1]) ;
return 2 ;
}
for(k = 2 ; k < argc ; k ++)
{
switch(k)
{
case 2 :
uiAvailableTimeInMiliSec = atoi(argv[k]) ;
break ;
case 3 :
nDesPos = atoi(argv[k]) ;
break ;
case 4 :
uiStartSpeed = atoi(argv[k]) ;
break ;
case 5 :
uiSpeedLimit = atoi(argv[k]) ;
break ;
case 6 :
uiAccLimit = atoi(argv[k]) ;
break ;
}
}
fprintf(fptr,"\n\t\tProfile Generation Algorithm Test Program\n") ;
fprintf(fptr,"\nSpeed Limit in Steps/Sec is:%u",uiSpeedLimit) ;
fprintf(fptr,"\nAcceleration Limit in Steps/Sec/Sec is:%u",uiAccLimit) ;
fprintf(fptr,"\nStart Speed in Steps/Sec is:%u",uiStartSpeed ) ;
fprintf(fptr,"\nDesired Position (Number of Steps) is:%u",nDesPos ) ;
fprintf(fptr,"\nAvailable time in mili seconds is:%u\n\n",uiAvailableTimeInMiliSec ) ;
RunMotor(uiStartSpeed,(uiAvailableTimeInMiliSec * 1000),nDesPos,uiSpeedLimit,uiAccLimit) ;
fclose(fptr) ;
return 0;
}