void main(void) {
uint16 speed = param_speed;
uint8 port = param_port,
pin=param_pin;
uint8 ServoPos = 120;
uint8 dir=1;
systemInit();
usbInit();
InitServos();
EA=1; // Global interrupt enabled
SetPin(0, port, pin);
SetPin(1, 0, 1);
SetPin(2, 0, 3);
while (1) {
delayMs(speed);
usbComService();
if (dir==1) {
++ServoPos;
}
else {
--ServoPos;
}
if (ServoPos > 253 || ServoPos < 2) {
dir ^=1;
}
SetPos(0,ServoPos);
SetPos(1,128);
SetPos(2,64);
}
}
void main(void) {
TRISB = 0;
TRISC = 1;
TRISD = 0;
PORTD = 0;
SetClockBase(CLOCK_20MHz);
SetPulseWidth(1000, 2000);
SetServoScale(0, 180);
posMin = GetMinPosition();
posMax = GetMaxPosition();
InitServos(4, (char) &PORTB);
MoveAllServos(180);
Delay10KTCYx(150);
MoveServoAsync(6, 5);
Delay10KTCYx(150);
MoveServoAsync(6, 5);
}
/* ----------------------------------------------------- */
void TestServo(void)
{
WriteLine("Initializing the servos...");
InitServos();
WriteLine("Powering on the servos...");
Power_On_Servos();
WriteLine("Loading the start positions...");
LoadStartPosition();
_delay_ms(2000);
MoveServo(1, 700);
_delay_ms(2000);
MoveServo(1, 1500);
_delay_ms(2000);
MoveServo(2, 700);
_delay_ms(2000);
MoveServo(2, 1500);
_delay_ms(2000);
MoveServo(3, 700);
_delay_ms(2000);
MoveServo(3, 1500);
_delay_ms(2000);
MoveServo(4, 700);
_delay_ms(2000);
MoveServo(4, 1500);
_delay_ms(2000);
MoveServo(5, 700);
_delay_ms(2000);
MoveServo(5, 1500);
_delay_ms(2000);
MoveServo(6, 700);
_delay_ms(2000);
MoveServo(6, 1500);
_delay_ms(2000);
Power_Off_Servos();
}
void ServoDriverLininoSetServos(UINT32 servoc, UINT32 *servov)
{
if(!isInit)
{
InitServos();
isInit=TRUE;
}
UINT s=0;
BOOL servoChanged = FALSE;
static UINT count;
UINT update = (count++ % 50);
for(s=0; s < MAX_SERVOS; s++)
{
if(s < servoc )
{
// update channel if position changed or 1s interval if servo is not disabled
if((oldChannels[s] != servov[s]) || ((update == s) && servov[s]))
{
SetServo(s, servov[s]);
oldChannels[s] = servov[s];
servoChanged = TRUE;
}
}
else
{
if(oldChannels[s] != 0)
{
SetServo(s, 0);
oldChannels[s] = 0;
servoChanged = TRUE;
}
}
}
if(servoChanged)
PrintServoStates();
}
/*-------------------------------------------------------------------
* UserMain
*-----------------------------------------------------------------*/
void UserMain(void *pd)
{
SimpleUart(0,115200);
assign_stdio(0);
iprintf("Before init stack\r\n");
InitializeStack();
{
WORD ncounts = 0;
while ( ( !bEtherLink ) && ( ncounts < 2*TICKS_PER_SECOND ) )
{
ncounts++;
OSTimeDly( 1 );
}
}
EnableAutoUpdate();
EnableTaskMonitor();
OSChangePrio( MAIN_PRIO ); // set standard UserMain task priority
//BcastSysLogPrintf("Application built on %s on %s\r\nWait...", __TIME__, __DATE__ );
pUserUdpProcessFunction=MyUDPProcessFunction;
// UdpCheck();
OSSemInit(&DataSem,0);
LoadSensorConfig();
// BcastSysLogPrintf("1");
InitLog();
ImuInit(IMU_PRIO,&DataSem);
InitIDSM2SubSystem(RC_PRIO,&DataSem);
InitGpsSubSystem(GPS_PRIO,&DataSem);
InitServos();
SetServo(0,0);
SetServo(1,0);
SetServo(2,0);
SetServo(3,0);
WORD LastGps=GPS_Result.ReadingNum;
WORD LastRc =DSM2_Result.ReadingNum;
WORD LastImu =IMU_Result.ReadingNum;
DWORD LSecs=Secs;
static short mmx;
static short mmy;
static short nmx;
static short nmy;
static short mgz;
static short ngz;
DWORD ZeroTime=Secs+5;
long GZSum=0;
int GZCnt=0;
float fmh=0.0;
while(ZeroTime > Secs)
{
OSSemPend(&DataSem,20);
if(LastImu !=IMU_Result.ReadingNum)
{
GZSum+=IMU_Result.gz;
GZCnt++;
LastImu =IMU_Result.ReadingNum;
if (IMU_Result.mx !=0 )
fmh=CalcMagHeading(IMU_Result.mx,IMU_Result.my);
}
}
short gzoff=(GZSum/GZCnt);
float fIheading=fmh;
while(1)
{
OSSemPend(&DataSem,20);
if (LastGps!=GPS_Result.ReadingNum )
{
LastGps=GPS_Result.ReadingNum;
LogSmGps(GPS_Result);
}
if (LastRc !=DSM2_Result.ReadingNum)
{
WORD w=DSM2_Result.val[1];
float f=DSM_Con(w);
SetServo(STEER_CH,f);
w=DSM2_Result.val[0];
f=DSM_Con(w);
SetServo(THROTTLE_CH,f);
LastRc =DSM2_Result.ReadingNum;
// LogRC(DSM2_Result);
}
if(LSecs!=Secs)
{
// BcastSysLogPrintf("Tick %d Iat:%ld lon:%ld SAT:%d\r\n",Secs,GPS_Result.LAT,GPS_Result.LON,GPS_Result.numSV);
//SysLogPrintf(ipa_syslog_addr,514,
LogMaxMin(mgz,mmx,mmy,ngz,nmx,nmy);
//static char tbuf[256];
// siprintf(tbuf,"TCN=%ld\r\n",sim.timer[0].tcn);
// writestring(LOG_UART,tbuf);
mgz=IMU_Result.mz;
mmx=IMU_Result.mx;
mmy=IMU_Result.my;
ngz=IMU_Result.mz;
nmx=IMU_Result.mx;
nmy=IMU_Result.my;
LSecs=Secs;
}
if(LastImu !=IMU_Result.ReadingNum)
{
if(IMU_Result.gz<ngz) ngz=IMU_Result.gz;
if(IMU_Result.gz>mgz) mgz=IMU_Result.gz;
fIheading+=(fGZHeadingScale_deg*(IMU_Result.gz-gzoff));
if(fIheading > 180.0 ) fIheading-=360.0;
if(fIheading <-180.0 ) fIheading+=360.0;
if (IMU_Result.mx !=0 )
{
if(IMU_Result.mx<nmx) nmx=IMU_Result.mx;
if(IMU_Result.mx>mmx) mmx=IMU_Result.mx;
if(IMU_Result.my<nmy) nmy=IMU_Result.my;
if(IMU_Result.my>mmy) mmy=IMU_Result.my;
volatile ImuRegisters il;
OSLock();
il.ax=IMU_Result.ax;
il.ay=IMU_Result.ay;
il.az=IMU_Result.az;
il.gx=IMU_Result.gx;
il.gy=IMU_Result.gy;
il.gz=IMU_Result.gz;
il.mx=IMU_Result.mx;
il.my=IMU_Result.my;
il.mz=IMU_Result.mz;
il.t =IMU_Result.t ;
il.ReadingNum=IMU_Result.ReadingNum;
OSUnlock();
fmh=CalcMagHeading(IMU_Result.mx,IMU_Result.my);
CorrectHeading(fIheading,fmh,0.005);//20 times per second correct in 10 seconds so 0.005
il.fIhead=fIheading;
il.fMhead=fmh;
il.GHeading= GPS_Result.Heading;
il.odo=sim.timer[0].tcn;
LogImu(il);
}
LastImu =IMU_Result.ReadingNum;
}
}//While
}
int main(void)
{
DDRD = 0b01110000;
DDRA = 0b00000001;
//wdt_reset();
//wdt_disable();
char replyDat[2];
i2c_init();
USART roverPort;
USART devicePort;
CommInterface inf;
CommPacket commPkt;
char commData[20];
char commRet;
Timer accelTimer;
Timer dataTimer;
int sensorError;
//PORTA = 1;
//_delay_ms(250);
//PORTA = 0;
//_delay_ms(250);
USART_InitPortStructs();
setDeviceCamera();
USART_Open(&roverPort, 0, USART_BAUD_38400, 20, 32, true);
CommInterfaceInit(&inf, &roverPort);
USART_Open(&devicePort, 1, USART_BAUD_9600, 20, 5, false);
setDeviceServo();
sensorError = InitAccelerometer(&inf);
sensorError = InitBarometer(&inf);
InitServos(&devicePort);
InitCamera(&devicePort);
InitTimers();
//StartTimer(&accelTimer);
//StartTimer(&dataTimer);
setDeviceServo();
PORTA |= 1;
PanTiltSetPosition(TILT_SERVO, TILT_CENTER);
PanTiltSetPosition(PAN_SERVO, PAN_CENTER);
char pingRespond = 0xAA;
commPkt.data = commData;
CommPacket pkt;
char textBuffer[40];
char length;
unsigned char aclBuf[6];
//ServiceBarometer(3);
//Enable Watchdog
//wdt_enable(0b111);
while(1) {
while(CommRXPacketsAvailable(&inf)) {
commRet = CommGetPacket(&inf, &commPkt, 20);
if (!commRet) // didn't get packet returned, even though it said we had one. give up for now
break;
if (commPkt.length==0) { // bad 0-length packet, skip to the next one
continue;
}
// must at least have a length of 1, check the first byte to see what device this is targeted at
if (commPkt.data[0]==SYS_PANTILT) {
PORTA &= ~1;
setDeviceServo();
PanTiltHandlePacket(&commPkt);
PORTA = sensorError;
}
else if (commPkt.data[0]==SYS_CAMERA) {
PORTA &= ~1;
setDeviceCamera();
CameraHandlePacket(&commPkt);
PORTA = sensorError;
}
else if (commPkt.data[0]==SYS_BAROMETER) {
PORTA &= ~1;
BarometerHandlePacket(&commPkt);
PORTA = sensorError;
}
else if (commPkt.data[0]==SYS_ACCEL) {
PORTA &= ~1;
AccelHandlePacket(&commPkt);
PORTA = sensorError;
}
else if (commPkt.data[0]==SYS_PING)
{
pkt.target = 1;
pkt.length = 1;
pkt.data = &pingRespond;
CommSendPacket(&inf,&pkt);
}
}
/*
if (GetSpanUs(&dataTimer) > FROM_uS(1000000))
{
USART_WriteByte(&roverPort,0xFE);
USART_WriteByte(&roverPort,temperatureData>>8);
USART_WriteByte(&roverPort,temperatureData&0xFF);
USART_WriteByte(&roverPort,barometerData>>16);
USART_WriteByte(&roverPort,(barometerData>>8)&0xFF);
USART_WriteByte(&roverPort,barometerData&0xFF);
AccelGetAverage(aclBuf);
length = sprintf(textBuffer,"X: %d \tY: %d \n\r",(aclBuf[0]<<8)|aclBuf[1], (aclBuf[2]<<8)|aclBuf[3]);
USART_Write(&roverPort,textBuffer,length);
StartTimer(&dataTimer);
}
*/
/*
if (GetSpanUs(&accelTimer) > 39)
{
sensorError |= AccelAddDataToBuffer();
StartTimer(&accelTimer);
}
if (BarometerGetState() == 1)
{
sensorError |= ServiceBarometer(3);
}*/
}
}
//****************************************************************************
// MAIN FUNCTION
//****************************************************************************
void main()
{
long lRetVal = 0;
char data[BUF_SIZE];
char sent_data[BUF_SIZE];
unsigned char highByte, lowByte;
int i;
unsigned long tempStartTimeStamp, tempStopTimeStamp;
memset(sent_data, 0, 10);
// Board Initialization
BoardInit();
// Configure the pinmux settings for the peripherals exercised
PinMuxConfig();
// Initialize the PWM outputs on the board
InitServos();
// Initialize the sensor ADC
InitSensorADC();
// Configuring UART
InitTerm();
// Display banner
DisplayBanner(APPLICATION_NAME);
// Connect to WIFI using default info
//WlanConnect(NULL, NULL, NULL);
WlanConnect("Nagui's Network", "SL_SEC_TYPE_WPA", "19520605");
// Setup the TCP Server Socket
BsdTcpServerSetup(PORT_NUM);
// Recieve Data
while (lRetVal >= 0)
{
lRetVal = BsdTcpServerReceive(data);
//SysTickPeriodSet(800000000); // test
//SysTickEnable(); // test
Timer_IF_Init(PRCM_TIMERA0, TIMERA0_BASE, TIMER_CFG_PERIODIC, TIMER_A, 0);
Timer_IF_Start(TIMERA0_BASE, TIMER_A, MILLISECONDS_TO_TICKS(2000));
//tempStartTimeStamp = SysTickValueGet(); // test
tempStartTimeStamp = Timer_IF_GetCount(TIMERA0_BASE, TIMER_A);
for (i = 0; i<NUM_SERVOS; i++)
{
MoveServo((unsigned char)data[i], (enum Servo_Joint_Type)i);
UART_PRINT("%d", (unsigned int) data[i]);
}
for (i = 0; i< NUM_SENSORS; i++)
{
//UnsignedShort_to_UnsignedChar(GetSensorReading((enum Fingertip_Sensor_Type)i), &highByte, &lowByte);
UnsignedShort_to_UnsignedChar(GetSensorReading(SENSOR_FINGER_INDEX), &highByte, &lowByte);
sent_data[i*2] = (char)highByte;
sent_data[i*2+1] = (char)lowByte;
}
//tempStopTimeStamp = SysTickValueGet(); // test
tempStopTimeStamp = Timer_IF_GetCount(TIMERA0_BASE, TIMER_A);
lRetVal = BsdTcpServerSend(sent_data, 10);
UART_PRINT("timestamp start: %lu\n\r", tempStartTimeStamp); //test
UART_PRINT("timestamp stop: %lu\n\r", tempStopTimeStamp); //test
UART_PRINT("Sent 10 bytes to client.\n\r");
}
UART_PRINT("Exiting Application ...\n\r");
// power of the Network processor
lRetVal = sl_Stop(SL_STOP_TIMEOUT);
}
