void GyroCalibration()
{
const char axis_done = GetChar();
const char axis = axis_done - 0x78;
const char calibration_done = 'd';
signed char status;
nvtCalGyroInit(axis);
do {
SensorsRead(SENSOR_GYRO,1);
#ifndef OPTION_RC
DelayMsec(5);
#else
DelayMsec(16);
#endif
//printf("T:%d\n",getTickCount());
status=nvtGyroScaleCalibrate(axis);
led_arm_state(LED_STATE_TOGGLE);
UpdateLED();
} while(status==STATUS_GYRO_CAL_RUNNING);
if(status==STATUS_GYRO_AXIS_CAL_DONE)
Serial_write((char*)&axis_done, 1);
else {
Serial_write((char*)&calibration_done, 1);
UpdateFlashCal(SENSOR_GYRO, false);
}
}
int main() {
char clkwise=1;
initPort();
while(1) {
int i=0,j=0;
if (clkwise) {
for (i=0;i<=7;i++)
{
for (j=0;j<5;j++)
{
PORTDCLR = 0x0002;
PORTDSET = 0x0010;
DelayMotor();
PORTDCLR = 0x0010;
PORTDSET = 0x0008;
DelayMotor();
PORTDCLR = 0x0008;
PORTDSET = 0x0004;
DelayMotor();
PORTDCLR = 0x0004;
PORTDSET = 0x0002;
DelayMotor();
}
DelayMsec(30);
}
}
else {
DelayMsec(30);
for (i=0;i<=7;i++)
{
for (j=0;j<5;j++)
{
PORTDCLR = 0x0010;
PORTDSET = 0x0002;
DelayMotor();
PORTDCLR = 0x0002;
PORTDSET = 0x0004;
DelayMotor();
PORTDCLR = 0x0004;
PORTDSET = 0x0008;
DelayMotor();
PORTDCLR = 0x0008;
PORTDSET = 0x0010;
DelayMotor();
}
DelayMsec(30);
}
}
}
}
void AccCalibration()
{
const char direction = GetChar();
const char calibration_done = 'd';
char side = atoi(&direction);
signed char status;
if (direction == 'x')
side = 0;
else
side = atoi(&direction);
if ((direction == '0')||(direction == 'x'))
nvtCalACCInit();
do {
DelayMsec(1);
SensorsRead(SENSOR_ACC,1);
status = nvtCalACCBufferFill(side);
}while(status==STATUS_BUFFER_NOT_FILLED);
if(status==STATUS_BUFFER_FILLED) {
if (direction == 'x')
UpdateFlashCal(SENSOR_ACC, false);
Serial_write((char*)&direction, 1);
}
else {
Serial_write((char*)&calibration_done, 1);
UpdateFlashCal(SENSOR_ACC, false);
}
}
void GyroCalibration()
{
const char axis_done = GetChar();
const char axis = axis_done - 0x78;
const char calibration_done = 'd';
signed char status;
nvtCalGyroInit(axis);
do
{
SensorsRead(SENSOR_GYRO,1);
DelayMsec(16);
status=nvtGyroScaleCalibrate(axis);
}
while(status==STATUS_GYRO_CAL_RUNNING);
if(status==STATUS_GYRO_AXIS_CAL_DONE)
Serial_write((char*)&axis_done, 1);
else
{
Serial_write((char*)&calibration_done, 1);
UpdateFlashCal(SENSOR_GYRO, false);
}
}
void LCD_puts(const uchar *s) {
uchar *p=s;
while (*p) {
RS=1;
LCD_putchar(*p);
DelayMsec(5);
RS=0;
p++;
}
}
void MagCalibration()
{
char calibration_done;
signed char status;
int16_t RawMAG[3];
nvtCalMAGInit();
do {
#ifndef OPTION_RC
DelayMsec(160);
#else
DelayMsec(320);
#endif
SensorsRead(SENSOR_MAG,1);
status = nvtCalMAGBufferFill();
nvtGetSensorRawMAG(RawMAG);
if (report_format == REPORT_FORMAT_BINARY) {
Serial_write((char*)RawMAG, 6);
}
else if (report_format == REPORT_FORMAT_TEXT) {
printf("@rM:%d,%d,%d\n",RawMAG[0],RawMAG[1],RawMAG[2]);
}
}while(status==STATUS_BUFFER_NOT_FILLED);
if(status==STATUS_CAL_DONE) {
CalQFactor = nvtGetMagCalQFactor();
if(CalQFactor<MAG_CAL_SUCESS_TH)
calibration_done = 'd';
else
calibration_done = 'f';
if (report_format == REPORT_FORMAT_BINARY) {
Serial_write((char*)&calibration_done, 1);
Serial_write((char*)&CalQFactor, 1);
}
else if (report_format == REPORT_FORMAT_TEXT) {
printf("%c,%d\n",calibration_done,CalQFactor);
}
}
}
main(){
TRISC = 0xfff0;
TRISD = 0x0000;
LCD_init();
//*Test for LCD*/
/*-----------------
LCD_puts(startStr1);
LCD_goto(0x40);
LCD_puts(startStr2);
while (1) {}
//*/
///*-----------------
initADC();
while(1) {
AD1CON1SET=0x0002;
DelayMsec(2);
AD1CON1CLR=0x0002;
while (!(AD1CON1 & 0x0001));
ADCValue = ADC1BUF0;
double Vtg=3.3*ADCValue/1024;
int x=100*Vtg;
int x1=x/100; x-=x1*100;
int x2=x/10; x-=x2*10;
int x3=x;
uchar startStr4[14];
startStr4[0]=x1+48;
startStr4[1]='.';
startStr4[2]=x2+48;
startStr4[3]=x3+48;
startStr4[4]='V';
startStr4[5]=' ';
startStr4[6]=' ';
startStr4[7]=' ';
startStr4[8]=' ';
startStr4[9]=' ';
startStr4[10]=' ';
startStr4[11]=' ';
startStr4[12]=' ';
startStr4[13]=' ';
LCD_puts(startStr1);
LCD_goto(0x40);
LCD_puts(startStr4);
}//while
//*/
}//main
void setupGPS()
{
uint8_t i;
//GPS_write(UBLOX_57600,sizeof(UBLOX_57600),5);
//UART_Open(UART1, 57600);
//DelayMsec(200);
//GPS_write(UBLOX_INIT, sizeof(UBLOX_INIT),5);
setupGpsUART();
for(i=0; i<5; i++) {
GPS_NewData();
DelayMsec(100);
}
if(GPS_Info.GPS_Present)
printf("GPS OK\n");
else
printf("GPS ERROR\n");
}
void LCD_init() {
DelayMsec(8); //wait for 15 ms
RS = 0; //send command
Data = LCD_IDLE; //function set - 8 bit interface
DelayMsec(8); //wait for 5 ms
Data = LCD_IDLE; //function set - 8 bit interface
DelayMsec(8); //wait for 100 us
Data = LCD_IDLE; //function set
DelayMsec(8);
Data = LCD_IDLE;
DelayMsec(8);
LCD_putchar(LCD_2_LINE_4_BITS);
DelayMsec(8);
LCD_putchar(LCD_DSP_CSR);
DelayMsec(5);
LCD_putchar(LCD_CLR_DSP);
DelayMsec(6);
LCD_putchar(LCD_CSR_INC);
}
void SensorInitBARO()
{
#ifdef BMP085
SensorInitState.BARO_Done = begin(BMP085_ULTRAHIGHRES);
if(SensorInitState.BARO_Done)
SensorInitState.BARO_BRAND = BMP085;
#endif
#ifdef BMP280
SensorInitState.BARO_Done = Int_BMP280();
if(SensorInitState.BARO_Done) {
SensorInitState.BARO_BRAND = BMP280;
printf("Baro Sensor - [BMP280]\n");
}
else
printf("Baro Sensor - [NA]\n");
#endif
if(SensorInitState.BARO_Done) {
switch (SensorInitState.BARO_BRAND) {
#ifdef BMP085
case BMP085:
TriggerRawPressure();
DelayMsec(24);
SensorInitState.BARO_BasePressure = readRawPressure();
TriggerRawTemperature();
BaroDoTick = getTickCount() + 15;
BaroDoState = 0;
Sensor.BaroInfo.baroPressureSum = 0;
break;
#endif
#ifdef BMP280
case BMP280:
{
bool isBMP280TestPassed = BMP280SelfTest();
printf("Baro Test Passed:%d\n",isBMP280TestPassed);
}
break;
#endif
}
printf("BARO connect - [OK]\n");
}
else
printf("BARO connect - [FAIL]\n");
}
void setup()
{
uint8_t i=0;
//初始化系统时钟
setupSystemClock();
//初始化串口
setupUART();
UART_NVIC_INIT();
//初始化System_tick
setup_system_tick(SYSTEM_TICK_FREQ);
//初始化IIC
I2C_Init();
//初始化FLASH
FlashInit();
LoadParamsFromFlash();
//初始化低电压检测
BatteryCheckInit();
//初始化遥控
Comm_Init();
//初始化LED
LED_Init();
//初始化SENSOR
#ifdef IMU_SW //软件姿态解算
// MPU6050_initialize();
// DelayMsec(1000); //必须加延迟，否则读取陀螺仪数据出错
#else
MPU6050_initialize();
DelayMsec(1000); //必须加延迟，否则读取陀螺仪数据出错
MPU6050_DMP_Initialize(); //初始化DMP引擎
#endif
//初始化自稳定
// LED_ON();
// //测试用，延迟启动时间
// for(i=0;i<6;i++)
// {
// DelayMsec(1000);
// LED_OFF();
// }
//初始化电机
Motor_Init();
//printf("Motor_Init(); \n");
//IMU_Init(); // sample rate and cutoff freq. sample rate is too low now due to using dmp.
// printf("\n\nCPU @ %dHz\n", SystemCoreClock);
//MotorPwmOutput(100,100,0,0);
}
int main() {
a=0;b=0;c=0;
a1=0;b1=0;c1=0;
first=1;
start=0;
int k=0;
for (k=0;k<=7;k++) {ref_distance1[k]=0;ref_distance2[k]=0;cur_distance1[k]=0;cur_distance2[k]=0;}
initIntGlobal();
initCN();
initTimer2();
initTimer4();
initPort();
LCD_init();
while(1)
{
while(start)
{
int i=0,j=0;
for (i=1;i<=8;i++)
{
for (j=0;j<32;j++)
{
PORTDCLR = 0x0002;
PORTDSET = 0x0010;
DelayMotor();
PORTDCLR = 0x0010;
PORTDSET = 0x0008;
DelayMotor();
PORTDCLR = 0x0008;
PORTDSET = 0x0004;
DelayMotor();
PORTDCLR = 0x0004;
PORTDSET = 0x0002;
DelayMotor();
}
///////////////////////////////////////////////////////////////////////////////////////////
TMR2=0; TMR4=0;
PORTDSET=0x0021;
DelayUsec(15);
PORTDCLR=0x0021;
DelayMsec(16);
a=TMR2; a1=TMR4;
TMR2=0; TMR4=0;
PORTDSET=0x0021;
DelayUsec(15);
PORTDCLR=0x0021;
DelayMsec(16);
b=TMR2; b1=TMR4;
TMR2=0; TMR4=0;
PORTDSET=0x0021;
DelayUsec(15);
PORTDCLR=0x0021;
DelayMsec(16);
c=TMR2; c1=TMR4;
//DisplayTMR2();
//DisplayTMR4();
if ( (a<80000 || a1<80000 || b<80000 || b1<80000 || c<80000 || c1<80000)&& (b-c>500
|| c-b>500 || b1-c1>500 || c1-b1>500 || a-b>500 || b-a>500 ||
a1-b1>500 || b1-a1>500 || a-c>500 || c-a>500 || a1-c1>500 || c1-a1>500 ))
{PORTDbits.RD7=1;}
DelayMsec(20);
PORTDbits.RD7=0;
///////////////////////////////////////////////////////////////////////////////////////////
}
first=0;
for (i=7;i>=0;i--)
{
for (j=0;j<32;j++)
{
PORTDCLR = 0x0010;
PORTDSET = 0x0002;
DelayMotor();
PORTDCLR = 0x0002;
PORTDSET = 0x0004;
DelayMotor();
PORTDCLR = 0x0004;
PORTDSET = 0x0008;
DelayMotor();
PORTDCLR = 0x0008;
PORTDSET = 0x0010;
DelayMotor();
}
///////////////////////////////////////////////////////////////////////////////////////////
TMR2=0; TMR4=0;
PORTDSET=0x0021;
DelayUsec(15);
PORTDCLR=0x0021;
DelayMsec(16);
a=TMR2; a1=TMR4;
TMR2=0; TMR4=0;
PORTDSET=0x0021;
DelayUsec(15);
PORTDCLR=0x0021;
DelayMsec(16);
b=TMR2; b1=TMR4;
TMR2=0; TMR4=0;
PORTDSET=0x0021;
DelayUsec(15);
PORTDCLR=0x0021;
DelayMsec(16);
c=TMR2; c1=TMR4;
//DisplayTMR2();
//DisplayTMR4();
if ( (a<80000 || a1<80000 || b<80000 || b1<80000 || c<80000 || c1<80000)&& (b-c>500
|| c-b>500 || b1-c1>500 || c1-b1>500 || a-b>500 || b-a>500 ||
a1-b1>500 || b1-a1>500 || a-c>500 || c-a>500 || a1-c1>500 || c1-a1>500 ))
{PORTDbits.RD7=1;}
DelayMsec(20);
PORTDbits.RD7=0;
///////////////////////////////////////////////////////////////////////////////////////////
}
}
}
}
int main() {
initPWM();
a=0;b=0;c=0;
a1=0;b1=0;c1=0;
start=0;
first=1;
alm=0;
motor_position=0;
initIntGlobal();
initCN();
initTimer2();
initTimer4();
initPort();
LCD_init();
while(1)
{
while (start)
{
int i=0,j=0;
for (i=1;i<=8;i++)
{
motor_position=i;
for (j=0;j<32;j++)
{
PORTDCLR = 0x0002;
PORTDSET = 0x0010;
DelayMotor();
PORTDCLR = 0x0010;
PORTDSET = 0x0008;
DelayMotor();
PORTDCLR = 0x0008;
PORTDSET = 0x0004;
DelayMotor();
PORTDCLR = 0x0004;
PORTDSET = 0x0002;
DelayMotor();
}
///////////////////////////////////////////////////////////////////////////////////////////
TMR2=0; TMR4=0;
PORTDSET=0x0220;
DelayUsec(15);
PORTDCLR=0x0220;
DelayMsec(16);
a=TMR2; a1=TMR4;
TMR2=0; TMR4=0;
PORTDSET=0x0220;
DelayUsec(15);
PORTDCLR=0x0220;
DelayMsec(16);
b=TMR2; b1=TMR4;
TMR2=0; TMR4=0;
PORTDSET=0x0220;
DelayUsec(15);
PORTDCLR=0x0220;
DelayMsec(16);
c=TMR2; c1=TMR4;
//DisplayTMR2();
//DisplayTMR4();
if ( (a<80000 || a1<80000 || b<80000 || b1<80000 || c<80000 || c1<80000)&& (b-c>800
|| c-b>800 || b1-c1>800 || c1-b1>800 || a-b>800 || b-a>800 ||
a1-b1>800 || b1-a1>800 || a-c>800 || c-a>800 || a1-c1>800 || c1-a1>800 ))
{alarm();
break;}
//DelayMsec(20);
///////////////////////////////////////////////////////////////////////////////////////////
}
if (alm) {alm=0;break;}
for (i=7;i>=0;i--)
{
motor_position=i;
for (j=0;j<32;j++)
{
PORTDCLR = 0x0010;
PORTDSET = 0x0002;
DelayMotor();
PORTDCLR = 0x0002;
PORTDSET = 0x0004;
DelayMotor();
PORTDCLR = 0x0004;
PORTDSET = 0x0008;
DelayMotor();
PORTDCLR = 0x0008;
PORTDSET = 0x0010;
DelayMotor();
}
///////////////////////////////////////////////////////////////////////////////////////////
TMR2=0; TMR4=0;
PORTDSET=0x0220;
DelayUsec(15);
PORTDCLR=0x0220;
DelayMsec(16);
a=TMR2; a1=TMR4;
TMR2=0; TMR4=0;
PORTDSET=0x0220;
DelayUsec(15);
PORTDCLR=0x0220;
DelayMsec(16);
b=TMR2; b1=TMR4;
TMR2=0; TMR4=0;
PORTDSET=0x0220;
DelayUsec(15);
PORTDCLR=0x0220;
DelayMsec(16);
c=TMR2; c1=TMR4;
//DisplayTMR2();
//DisplayTMR4();
if ( (a<80000 || a1<80000 || b<80000 || b1<80000 || c<80000 || c1<80000)&& (b-c>800
|| c-b>800 || b1-c1>800 || c1-b1>800 || a-b>800 || b-a>800 ||
a1-b1>800 || b1-a1>800 || a-c>800 || c-a>800 || a1-c1>800 || c1-a1>800 ))
{alarm();
break;
}
//DelayMsec(20);
///////////////////////////////////////////////////////////////////////////////////////////
}
//if (alm) {alm=0;break;}
}
}
}
void RX_ISR(void)
{
received = U1ARXREG;
PORTD = 2;
if(received == 10)
{
i = 0;
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -2;
}
TMR2 = 0;
while(username[i] != NULL)
{
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = username[i];
}
i++;
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -2;
}
}
DelayMsec(5000);
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -3;
}
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -2;
}
i = 0;
while(password[i] != NULL)
{
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = password[i];
}
i++;
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -2;
}
}
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -3;
}
}
else if(received == -7)
{
if(receive_user == -1)
receive_user = 1;
else if(receive_user == 1)
receive_user = 0;
else
receive_user = -1;
i = 0;
}
else if(received == -4)
{
receive_file = 1;
file_pt = 0;
}
else if(received == -5)
{
receive_file = 0;
file_pt = 0;
//BYTE Key[2] = {'1','2'};
BYTE RoundKey[6];
Extend_Key(Key,RoundKey);
while(file[file_pt] != NULL)
{
BYTE temp[2];
BYTE temp_result[2];
temp[0] = file[file_pt];
temp[1] = file[file_pt+1];
EncryptBlock(temp,RoundKey,temp_result);
result[file_pt] = temp_result[0];
result[file_pt+1] = temp_result[1];
file_pt = file_pt + 2;
}
file_pt = 0;
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -2;
}
while(result[file_pt] != NULL)
{
char to_send = result[file_pt];
file_pt++;
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = to_send;
}
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -2;
}
}
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -3;
}
file_pt = 0;
}
else if(received == -6)
{
receive_file = 0;
file_pt = 0;
//BYTE Key[2] = {'1','2'};
BYTE RoundKey[6];
Extend_Key(Key,RoundKey);
while(file[file_pt] != NULL)
{
BYTE temp[2];
BYTE temp_result[2];
temp_result[0] = file[file_pt];
temp_result[1] = file[file_pt+1];
DecryptBlock(temp_result,RoundKey,temp);
file[file_pt] = temp[0];
file[file_pt+1] = temp[1];
file_pt = file_pt + 2;
}
file_pt = 0;
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -2;
}
while(file[file_pt] != NULL)
{
char to_send = file[file_pt];
file_pt++;
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = to_send;
}
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -2;
}
}
TMR2 = 0;
while(TMR2 < 400)
{
U1ATXREG = -3;
}
file_pt = 0;
}
if(receive_file == 1 && received != -4 && received != -5)
{
file[file_pt] = received;
file_pt ++;
}
if(receive_user == 1 && received != -7)
{
username[i] = received;
i++;
}
else if(receive_user == 0 && received != -7)
{
password[i] = received;
i++;
}
PORTDINV = 2;
IFS0bits.U1RXIF = 0;
}
int16_t AK8975_getHeadingZ() {
I2C_writeByte(devAddr, AK8975_RA_CNTL, AK8975_MODE_SINGLE);
DelayMsec(8);
I2C_readBytes(devAddr, AK8975_RA_HZL, 2, buffer,0);
return (((int16_t)buffer[1]) << 8) | buffer[0];
}
int main() {
// Initialize Sockets and IP address containers
//
SOCKET serverSock, clientSock = INVALID_SOCKET;
IP_ADDR curr_ip, ip;
// Initialize buffer length variables
//
int rlen, sent, bytesSent;
// Initialize the Send/Recv buffers
//
char rbfr[10];
// Socket struct descriptor
//
struct sockaddr_in addr;
int addrlen = sizeof(struct sockaddr_in);
// System clock containers
//
unsigned int sys_clk, pb_clk;
// Initialize LED Variables:
// Setup the LEDs on the PIC32 board
// RD0, RD1 and RD2 as outputs
//
mPORTDSetPinsDigitalOut(BIT_0 | BIT_1 | BIT_2 );
mPORTDClearBits(BIT_0 | BIT_1 | BIT_2); // Clear previous LED status.
// Setup the switches on the PIC32 board as inputs
//
mPORTDSetPinsDigitalIn(BIT_6 | BIT_7 | BIT_13); // RD6, RD7, RD13 as inputs
// Setup the system clock to use CPU frequency
//
sys_clk = GetSystemClock();
pb_clk = SYSTEMConfigWaitStatesAndPB(sys_clk);
// interrupts enabled
INTEnableSystemMultiVectoredInt();
// system clock enabled
SystemTickInit(sys_clk, TICKS_PER_SECOND);
// Initialize TCP/IP
//
TCPIPSetDefaultAddr(DEFAULT_IP_ADDR, DEFAULT_IP_MASK, DEFAULT_IP_GATEWAY,
DEFAULT_MAC_ADDR);
if (!TCPIPInit(sys_clk)) return -1;
DHCPInit();
// Port to bind socket to
//
addr.sin_port = 6653;
addr.sin_addr.S_un.S_addr = IP_ADDR_ANY;
// Initialize TCP server socket
//
if((serverSock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) ==
SOCKET_ERROR) return -1;
// Ensure we bound to the socket. End Program if bind fails
//
if(bind(serverSock, (struct sockaddr*) &addr, addrlen ) ==
SOCKET_ERROR)
return -1;
// Listen to up to five clients on server socket
//
listen(serverSock, 5);
// We store our desired transfer paragraph
//
char myStr[] = "TCP/IP (Transmission Control Protocol/Internet Protocol) is "
"the basic communication language or protocol of the Internet. "
"It can also be used as a communications protocol in a private "
"network (either an intranet or an extranet). When you are set up "
"with direct access to the Internet, your computer is provided "
"with a copy of the TCP/IP program just as every other computer "
"that you may send messages to or get information from also has "
"a copy of TCP/IP. TCP/IP is a two-layer program. The higher "
"layer, Transmission Control Protocol, manages the assembling "
"of a message or file into smaller packets that are transmitted "
"over the Internet and received by a TCP layer that reassembles "
"the packets into the original message. The lower layer, "
"Internet Protocol, handles the address part of each packet so "
"that it gets to the right destination. Each gateway computer on "
"the network checks this address to see where to forward the "
"message. Even though some packets from the same message are "
"routed differently than others, they'll be reassembled at the "
"destination.\0";
// Chunk up our data
//
// Copy our string into our buffer
//
int tlen = strlen(myStr);
char tbfr1[tlen1+1];
// Loop forever
//
while(1) {
// Refresh TCIP and DHCP
//
TCPIPProcess();
DHCPTask();
// Get the machines IP address and save to variable
//
ip.Val = TCPIPGetIPAddr();
// DHCP server change IP address?
//
if(curr_ip.Val != ip.Val) curr_ip.Val = ip.Val;
// TCP Server Code
//
if(clientSock == INVALID_SOCKET) {
// Start listening for incoming connections
//
clientSock = accept(serverSock, (struct sockaddr*) &addr, &addrlen);
// Upon connection to a client blink LEDS.
//
if(clientSock != INVALID_SOCKET) {
setsockopt(clientSock, SOL_SOCKET, TCP_NODELAY,
(char*)&tlen, sizeof(int));
mPORTDSetBits(BIT_0); // LED1=1
DelayMsec(50);
mPORTDClearBits(BIT_0); // LED1=0
mPORTDSetBits(BIT_1); // LED2=1
DelayMsec(50);
mPORTDClearBits(BIT_1); // LED2=0
mPORTDSetBits(BIT_2); // LED3=1
DelayMsec(50);
mPORTDClearBits(BIT_2); // LED3=0
}
}
else {
// We are connected to a client already. We start
// by receiving the message being sent by the client
//
rlen = recvfrom(clientSock, rbfr, sizeof(rbfr), 0, NULL,
NULL);
// Check to see if socket is still alive
//
if(rlen > 0) {
// If the received message first byte is '02' it signifies
// a start of message
//
if (rbfr[0]==2) {
//mPORTDSetBits(BIT_0); // LED1=1
// Check to see if message begins with
// '0271' to see if the message is a a global reset
//
if(rbfr[1]==71) {
mPORTDSetBits(BIT_0); // LED1=1
DelayMsec(50);
mPORTDClearBits(BIT_0); // LED1=0
}
}
// If the received message starts with a second byte is
// '84' it signifies a initiate transfer
//
if(rbfr[1]==84){
mPORTDSetBits(BIT_2); // LED3=1
bytesSent = 0;
//sent = 0;
while (bytesSent < tlen){
memcpy(tbfr1, myStr+bytesSent, tlen1);
if (bytesSent > 1049){
tbfr1[tlen-bytesSent+1] = '\0';
send(clientSock, tbfr1, tlen-bytesSent+1, 0);
}
else{
tbfr1[tlen1] = '\0';
// Loop until we send the full message
//
send(clientSock, tbfr1, tlen1+1, 0);
}
bytesSent += tlen1;
DelayMsec(50);
}
mPORTDClearBits(BIT_2); // LED3=0
}
mPORTDClearBits(BIT_0); // LED1=0
}
// The client has closed the socket so we close as well
//
else if(rlen < 0) {
closesocket(clientSock);
clientSock = SOCKET_ERROR;
}
}
}
}
int main() {
first=1;
start=0;
int k=0;
for (k=0;k<=7;k++) {ref_distance1[k]=0;ref_distance2[k]=0;cur_distance1[k]=0;cur_distance2[k]=0;}
initIntGlobal();
initCN();
initTimer2();
initTimer4();
initPort();
LCD_init();
while(1)
{
while(start)
{
int i=0,j=0;
for (i=0;i<=7;i++)
{
for (j=0;j<32;j++)
{
PORTDCLR = 0x0002;
PORTDSET = 0x0010;
DelayMotor();
PORTDCLR = 0x0010;
PORTDSET = 0x0008;
DelayMotor();
PORTDCLR = 0x0008;
PORTDSET = 0x0004;
DelayMotor();
PORTDCLR = 0x0004;
PORTDSET = 0x0002;
DelayMotor();
}
TMR2=0; TMR4=0;
PORTDSET=0x0021;
DelayUsec(15);
PORTDCLR=0x0021;
DelayMsec(30);
DisplayTMR2();
DisplayTMR4();
if (!first)
{cur_distance1[i]=TMR2; cur_distance2[i]=TMR4;}
else {ref_distance1[i]=TMR2;ref_distance2[i]=TMR4;
cur_distance1[i]=TMR2; cur_distance2[i]=TMR4;}
if (cur_distance1[i]-ref_distance1[i]>300 || ref_distance1[i]-cur_distance1[i]>300
||cur_distance2[i]-ref_distance2[i]>300 || ref_distance2[i]-cur_distance2[i]>300)
{PORTDbits.RD7=1;}
DelayMsec(20);
PORTDbits.RD7=0;
}
first=0;
for (i=0;i<=7;i++)
{
for (j=0;j<32;j++)
{
PORTDCLR = 0x0010;
PORTDSET = 0x0002;
DelayMotor();
PORTDCLR = 0x0002;
PORTDSET = 0x0004;
DelayMotor();
PORTDCLR = 0x0004;
PORTDSET = 0x0008;
DelayMotor();
PORTDCLR = 0x0008;
PORTDSET = 0x0010;
DelayMotor();
}
TMR2=0; TMR4=0;
PORTDSET=0x0021;
DelayUsec(15);
PORTDCLR=0x0021;
DelayMsec(30);
DisplayTMR2();
DisplayTMR4();
cur_distance1[7-i]=TMR2; cur_distance2[7-i]=TMR4;
if (cur_distance1[7-i]-ref_distance1[i]>300 || ref_distance1[i]-cur_distance1[7-i]>300
||cur_distance2[7-i]-ref_distance2[i]>300 || ref_distance2[i]-cur_distance2[7-i]>300)
{PORTDbits.RD7=1;}
DelayMsec(20);
PORTDbits.RD7=0;
}
}
}
}
