void initRS485(void)
{
initTimer4();
PR4=TimeOut;
TRISBbits.TRISB13 = 0; // output: -read/write
TRISBbits.TRISB14 = 0; // output: TX
TRISBbits.TRISB15 = 1; // input: RX
PORTBbits.RB13 = 0; // RS485 auf recieve stellen
U1MODE = 0; // Clear UART1 mode register
U1STA = 0; // Clear UART1 status register
_RP14R = 0b00011; // U1TX output -> RP14 pin
_U1RXR = 15; // U1RX input <- RP15 pin
U1MODEbits.STSEL = 0; // 1-stop bit
U1MODEbits.PDSEL = 0; // No Parity, 8-data bits
U1MODEbits.ABAUD = 0; // Auto-Baud disabled
U1MODEbits.BRGH = 0; // Standard-Speed mode
U1BRG = (FCY/(16*BAUD_RATE_UART1))-1; // Calculate value of baud rate register
_U1RXIE = 0; // Disable Rx interruptrs
_U1TXIE = 0; // Disable Tx interruptrs
U1MODEbits.UARTEN = 1; // Enable UART1 module
U1STAbits.UTXEN = 1; // Enable UART1 transmit
}
void initTruck(){
//Setup Debugging Connection
initUART1();
//Check for any errors
checkErrorCodes();
//Setup GPS
initGPS();
initTimer4();
//Setup Input and Output
initPWM(0b11,0b11);
PWMOutputCalibration(1,HUNTER_TRUCK_STEERING_SCALE_FACTOR, HUNTER_TRUCK_STEERING_OFFSET);
PWMOutputCalibration(2,HUNTER_TRUCK_THROTTLE_SCALE_FACTOR, HUNTER_TRUCK_THROTTLE_OFFSET);
setThrottle(0);
setSteering(0);
//Setup Datalink
// initDataLink();
}
int main() {
SYSTEMConfigPerformance(10000000); //Does something with assembly to set clock speed
enableInterrupts(); //Make interrupt work
initLEDs();
initTimer1();
initTimer2();
initTimer3();
initTimer4();
initSwitch();
porta = PORTA;
portd = PORTD;
initLCD();
delay(500);
//printCharLCD(0);
testLCD();
state = InitState;
nextState = InitState;
while (1) {
switch (state) {
case RUN:
#ifdef _DEBUG_
LATDbits.LATD0=0;
LATDbits.LATD1=0;
LATDbits.LATD2=1;
#endif
LATGbits.LATG14=LedOFF; //TRD1
LATGbits.LATG12=LedON; //TRD2
moveCursorLCD(0,0);
printStringLCD("Running...");
if(counterCN==1){ // If the counter changed...
printTimeLCD(counter);
counterCN = 0;
}
if(AllowChange == 0){
delay(5000);
delay(5000);
delay(5000);
delay(5000);
AllowChange = 1;
}
if((ReqChange==1) && (AllowChange==1)){
state=WAIT1;
ReqChange=0;
AllowChange=0;
// CNCONAbits.ON = 1;
}
break;
case STOP:
#ifdef _DEBUG_
LATDbits.LATD0=1;
LATDbits.LATD1=0;
LATDbits.LATD2=0;
#endif
LATGbits.LATG14=LedON;
LATGbits.LATG12=LedOFF;
moveCursorLCD(0,0);
printStringLCD("Stopped");
if(AllowChange == 0){
delay(5000);
delay(5000);
delay(5000);
delay(5000);
AllowChange = 1;
}
if(AllowReset == 0){
delay(5000);
delay(5000);
delay(5000);
delay(5000);
AllowReset = 1;
}
if((ReqReset==1) && (AllowReset==1)){
AllowReset = 0;
ReqReset = 0;
counter = 0;
printTimeLCD(counter);
}
if((ReqChange==1) && (AllowChange==1)){
state=WAIT2;
ReqChange=0;
AllowChange=0;
// CNCONAbits.ON = 1;
}
break;
case WAIT1:
#ifdef _DEBUG_
LATDbits.LATD0=0;
LATDbits.LATD1=1;
LATDbits.LATD2=1;
#endif
T1CONbits.ON = 0; // Turn off counter
if(AllowChange == 0){
delay(5000);
delay(5000);
delay(5000);
delay(5000);
AllowChange = 1;
}
if((ReqChange==1) && (AllowChange==1)){
state=STOP;
ReqChange=0;
AllowChange=0;
ReqReset = 0; // So we don't reset as soon as we stop...
AllowReset = 0; // See previous line
// CNCONAbits.ON = 1;
}
break;
case WAIT2:
#ifdef _DEBUG_
LATDbits.LATD0=1;
LATDbits.LATD1=1;
LATDbits.LATD2=0;
#endif
T1CONbits.ON = 1; // Turn on counter
if(AllowChange == 0){
delay(5000);// The sad part is this didn't even work.
delay(5000);
delay(5000);
delay(5000);
AllowChange = 1;
}
if((ReqChange==1) && (AllowChange==1)){ // Button Changed and De-bouncing period is done
state=RUN;
ReqChange=0;
AllowChange=0;
//CNCONAbits.ON = 1;
}
break;
case InitState:// Only happens once
state=STOP;
clearLCD();
counter = 0;
printTimeLCD(counter);
T1CONbits.ON = 0;
TMR1 = 0;
break;
default:// Should never happen. How did you get here?
state=InitState;
ReqChange = 0;
AllowChange=0;
break;
}
}
return 0;
}
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;
}
}
}
}
int main(void)
{
SYSTEMConfigPerformance(SYS_CLOCK);
enableInterrupts();
initADC();
initTimer2(); //for PWM
initPWM();
initLCD();
initTimer4(); //for pwm
initSW();
int track = 1;
int SpeedR = 0;
int SpeedL = 0;
int ones = 0;
int dec = 0;
float voltage = 0;
//Actual main state machine loop
while(1)
{
Speed = ADC1BUF0;
moveCursorLCD(0,0);
voltage = Speed * ((double)3.3/1023);
ones = floor(voltage);
dec = (voltage - ones) * 10;
printCharLCD('0'+ones);
printCharLCD('.');
printCharLCD('0'+dec);
printCharLCD('V');
delayUs(10000); //10ms - Just to help the LCD not strobe too much
switch(state)
{
case wait_Press: //handled by ISR
break;
case debouncePress:
state = wait_Release;
break;
case wait_Release: //handled by ISR
break;
case debounceRelease:
if (track == 1){
state = forward;
}
if (track == 2){
state = idle2;
}
if (track == 3){
state = reverse;
}
if (track == 4){
state = idle1;
}
break;
case idle1:
track = 1;
motorsOff();
state = wait_Press;
break;
case forward:
track = 2;
if(Speed == 511){
SpeedR = 1023;
SpeedL = 1023;
}
else if(Speed < 511){
SpeedR = 1023;
SpeedL = (2*Speed);
}
else if(Speed > 511){
SpeedR = 2*(1023 - Speed);
SpeedL = 1023;
}
else if (Speed == 1023){
SpeedR = 0;
SpeedL = 1023;
}
else if (Speed == 0){
SpeedR = 1023;
SpeedL = 0;
}
RightMotor_Write(SpeedR);
LeftMotor_Write(SpeedL);
break;
case idle2:
track = 3;
motorsOff();
state = wait_Press;
break;
case reverse:
track = 4;
if(Speed == 511){
SpeedR = 1023;
SpeedL = 1023;
}
else if(Speed < 511){
SpeedR = 1023;
SpeedL = 2*Speed;
}
else if(Speed > 511){
SpeedR = 2*(1023 - Speed);
SpeedL = 1023;
}
else if (Speed == 1023){
SpeedR = 1023;
SpeedL = 0;
}
else if (Speed == 0){
SpeedR = 0;
SpeedL = 1023;
}
RightMotor_Write(0-SpeedR); //maybe it needs the zero to be interpreted as negative?
LeftMotor_Write(0-SpeedL);
break;
}
}
return 0;
}
void pathManagerInit(void) {
initTimer4();
initLED(0);
//Communication with GPS
initGPS();
initBatterySensor();
initAirspeedSensor();
initSPI(IC_DMA_PORT, DMA_CLOCK_KHZ, SPI_MODE1, SPI_BYTE, SPI_MASTER);
initInterchip(DMA_CHIP_ID_PATH_MANAGER);
//Communication with Altimeter
if (initAltimeter()){
float initialValue = 0;
while (initialValue == 0) initialValue = getAltitude();
calibrateAltimeter(initialValue);
}
//Initialize Home Location
home.altitude = 400;
home.latitude = RELATIVE_LATITUDE;
home.longitude = RELATIVE_LONGITUDE;
home.radius = 1;
home.id = -1;
//Initialize first path nodes
// PathData* node = initializePathNode();
// node->altitude = 10;
// node->latitude = 43.473662;
// node->longitude = -80.540019;
// node->radius = 5;
// appendPathNode(node);
// node = initializePathNode();
// node->altitude = 10;
// node->latitude = 43.473479;
// node->longitude = -80.540601;
// node->radius = 5;
// appendPathNode(node);
// node = initializePathNode();
// node->altitude = 10;
// node->latitude = 43.473718;
// node->longitude = -80.540837;
// node->radius = 5;
// appendPathNode(node);
// node = initializePathNode();
// node->altitude = 10;
// node->latitude = 43.473946;
// node->longitude = -80.540261;
// node->radius = 5;
// appendPathNode(node);
// node = initializePathNode();
// node->altitude = 10;
// node->latitude = 43.473685;
// node->longitude = -80.540073;
// node->radius = 5;
// appendPathNode(node);
#if DEBUG
char str[20];
sprintf(str, "PM:%d, AM:%d", sizeof(PMData),sizeof(AMData));
debug(str);
#endif
}
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;}
}
}
}
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(void) {
SYSTEMConfigPerformance(40000000); //System clock is 40MHz
//Because interrupts are good to have
enableInterrupts();
//Because ADC is good to have
initADC();
//Because a timer is good to have for PWM
initTimer2();
//Because PWM is good to have
initPWM();
// initMotorDirPins();
//Because the delayUs function is typically pretty good to have
initTimer4();
//Because UART is good to have for interfacing with our speech chip
initUART(40000000);
//Because a talking robot is the absolute best thing to have
//initSpeakJet();
initSevenSegment();
initSW();
int HelloBytes[] = {0, 0, 0, 0b01110110, 0b01111001, 0b00111000, 0b00111000, 0b00111111, 0, 0, 0};
int helloCounter = 2;
int myCounter = 0;
int timesInFwd = 0;
//sendSpeakjet(initByteArray);
while(1) {
if(millis >= 5000) //if it's been some seconds
{
millis = 0; //reset the timer
//tell speakjet to speak state here
sendSpeakjet(AllYour);
}
if(ScrollMillis >= 600)
{
ScrollMillis = 0;
helloCounter = ((helloCounter + 1) % 8);
sevenSeg_write(0, HelloBytes[WRAPPOS((helloCounter-3), 8)]);
sevenSeg_write(1, HelloBytes[WRAPPOS(helloCounter-2,8)]);
sevenSeg_write(2, HelloBytes[(helloCounter-1) % 8]);
sevenSeg_write(2, HelloBytes[WRAPPOS((helloCounter-1), 8)]);
sevenSeg_write(3, HelloBytes[(helloCounter)]);
}
switch(myState) {
case idle:
motorsOff();
break;
case init:
//Wait for a button press to start us up
motorsOff();
sendSpeakjet(BiteMy);
delayUs(3000000);
myState = backwards;
break;
case backwards:
timesInFwd = 0;
LeftMotor_Write(-800);
RightMotor_Write(-800);
if(leftPairReading < THRESHOLD && rightPairReading < THRESHOLD)
myState = pivot_right;
else if (rightPairReading < THRESHOLD)
myState = turnL_back;
else if (leftPairReading < THRESHOLD)
myState = turnR_back;
else
myState = backwards;
break;
case turnR_back:
turnRight_back(73); //makes the LEFT motor turn at near full power. Yes, it IS the left motor and not the right one.
turnLeft_back(300);
sampleIRPairs(&leftPairReading, &rightPairReading, 10);
// myState = (rightPairReading >= THRESHOLD) ? forward : turnR;
if (leftPairReading < THRESHOLD && rightPairReading < THRESHOLD) //both dark
myState = pivot_right;
else if (rightPairReading < THRESHOLD)
myState = turnR_back;
else if (leftPairReading < THRESHOLD)
myState = turnL_back;
else
myState = backwards;
break;
case turnL_back:
turnRight_back(300); //makes the LEFT motor turn at near full power. Yes, it IS the left motor and not the right one.
turnLeft_back(73);
sampleIRPairs(&leftPairReading, &rightPairReading, 10);
// myState = (rightPairReading >= THRESHOLD) ? forward : turnR;
if (leftPairReading < THRESHOLD && rightPairReading < THRESHOLD) //both dark
myState = pivot_right;
else if (rightPairReading < THRESHOLD)
myState = turnR_back;
else if (leftPairReading < THRESHOLD)
myState = turnL_back;
else
myState = backwards;
break;
case pivot_right:
// if(leftPairReading >= THRESHOLD) //left is light
// myState = forward;
// break;
RightMotor_Write(-650);
LeftMotor_Write(700);
if(rightPairReading > THRESHOLD)
myState = forward;
break;
case turnR:
// turnRight(512);
turnRight(73); //makes the LEFT motor turn at near full power. Yes, it IS the left motor and not the right one.
turnLeft(800);
sampleIRPairs(&leftPairReading, &rightPairReading, 10);
// myState = (rightPairReading >= THRESHOLD) ? forward : turnR;
if (leftPairReading < THRESHOLD && rightPairReading < THRESHOLD) //both dark
myState = turnR;
else if (rightPairReading < THRESHOLD)
myState = turnR;
else if (leftPairReading < THRESHOLD)
myState = turnL;
else
myState = forward;
break;
case turnL:
// turnLeft(512);
turnLeft(73); //makes RIGHT motor run at near full power. Yes, it IS the right motor, not the left one.
turnRight(800);
sampleIRPairs(&leftPairReading, &rightPairReading, 10);
// myState = (leftPairReading >= THRESHOLD) ? forward : turnL;
if (leftPairReading < THRESHOLD && rightPairReading < THRESHOLD)
myState = turnR;
else if (rightPairReading < THRESHOLD)
myState = turnR;
else if (leftPairReading < THRESHOLD)
myState = turnL;
else
myState = forward;
break;
case forward:
if(timesInFwd >= 25000)
{
sendSpeakjet(finishedPhrase);
myState = backwards;
// myState = idle;
break;
}
timesInFwd++;
goForward();
sampleIRPairs(&leftPairReading, &rightPairReading, 100);
if (rightPairReading < THRESHOLD) {
myState = turnR;
timesInFwd = 0;
}
else if (leftPairReading < THRESHOLD) {
myState = turnL;
timesInFwd = 0;
}
break;
}
}
return (EXIT_SUCCESS);
}
