void initFCRuntime (void) {
Y_Difference_old = 0;
Y_DifferenceTotal = 0;
Y_Difference_Sum = 0;
X_Difference_old = 0;
X_DifferenceTotal = 0;
X_Difference_Sum = 0;
DifferencePitchFact = 0;
Pitch_Difference_Sum = 0;
Pitch_Difference_old = 0;
DifferencePitchGyr = 0;
Pitch_Difference = 0;
Y_Response = 0;
Y_Gyr_ACC_Difference = 0;
Y_Difference_AccY = 0;
Y_Difference_GyrN = 0;
X_Response = 0;
X_Gyr_ACC_Difference = 0;
X_Difference_AccX = 0;
X_Difference_GyrR = 0;
Difference = 0;
X_DifferenceFact = 0;
Y_DifferenceFact = 0;
Engine_value = 0;
enginesOff();
initServos();
commandRetrived = 0;
SettingsReceived = 0;
ADC_mit[ADC_NICK] = 0;
ADC_mit[ADC_ACCY] = 0;
ADC_mit[ADC_ACCX] = 0;
ADC_mit[ADC_ACCZ] = 0;
ADC_mit[ADC_ROLL] = 0;
ADC_mit[ADC_PITCH] = 0;
ADC_mit[AIRPRESSURE] = 0;
ADC_mit[UBAT] = 0;
}
int main()
{
signal(SIGINT, &cleanup);
wiringPiSetup();
jandroid.motors = initMotors();
jandroid.servos = initServos();
jandroid.socket = init_socket();
while((jandroid.client_socket = add_client(jandroid.socket)) > 0)
{
printf("%s\n", "Listening for command");
listen_command(jandroid.motors, jandroid.servos, jandroid.client_socket);
}
cleanup(0);
return 0;
}
task main()
{
getJoystickSettings(joystick);
bool wait;
while (true)
{
wait = false;
while (wait==false)
{
motor[leftMotor] = joystick.joy1_y1*-1;
motor[rightMotor] = joystick.joy1_y2*-1;
wait1Msec(1);
if (joy1Btn(5))
{
wait = true;
}
else if (joy1Btn(6))
{
wait = true;
}
}
bool direction;
if (joy1Btn(5))
{
direction = true;
}
else if (joy1Btn(6))
{
direction = false;
}
joy1Btn(6);
initServos(direction);
}
}
int main(void)
{
unsigned int i = 0;
// Clock (80MHz)
SysCtlClockSet(SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
// GPIO
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1 | GPIO_PIN_2);
GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1, 0);
// UART (Serial)
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), 115200, (UART_CONFIG_PAR_NONE | UART_CONFIG_STOP_ONE | UART_CONFIG_WLEN_8));
UARTEnable(UART0_BASE);
UARTFIFODisable(UART0_BASE);
/*I2CInit();
// PCA9557
i2c_buff[0] = 0x03;
i2c_buff[1] = 0x00; // 0: Output 1: Input
I2CWrite(0x18, i2c_buff, 2); // IO Direction
i2c_buff[0] = 0x02;
i2c_buff[1] = 0x00;
I2CWrite(0x18, i2c_buff, 2); // IO Polarity
i2c_buff[0] = 0x01;
i2c_buff[1] = 0x8F;
I2CWrite(0x18, i2c_buff, 2); // Output H/L
*/
//initLEDs();
//initMotors();
//initEncoders();
initServos();
//initBluetooth();
//invertMotor(0);
//invertMotor(1);
//invertEncoder(0);
// Do some tests
//setMotor(0, 0.85);
//setMotor(1, 0.85);
setServoLimits(5, 0.35, 0.85);
// Enable Interrupts
IntMasterEnable();
while(1)
{
//for(i=0; i<12; i++)
//{
setServo(5, 0.0);
toggleRed();
SysCtlDelay(SysCtlClockGet());
// printf("%d\r\n", i*5);
setServo(5, 0.6);
toggleRed();
SysCtlDelay(SysCtlClockGet());
//}
// LED On
/* toggleRed();
printf("0:% 6ld 1:% 6ld\r\n", readEnc(0), readEnc(1));
SysCtlDelay(SysCtlClockGet() / 3 / 5);
if (i == 10) // 5 Seconds
{
i2c_buff[0] = 0x01;
i2c_buff[1] = 0x0F | 0x00;
I2CWrite(0x18, i2c_buff, 2); // Output H/L
}
i++;
*/
/*I2CMasterSlaveAddrSet(I2C0_BASE, 0x18, false); // Set Outputs Directions
I2CMasterDataPut(I2C0_BASE, 0x01);
I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_START);
while(I2CMasterBusy(I2C0_BASE));
I2CMasterDataPut(I2C0_BASE, 0x00);
I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH);
while(I2CMasterBusy(I2C0_BASE));
I2CMasterSlaveAddrSet(I2C0_BASE, 0x18, false); // Set Outputs Directions
I2CMasterDataPut(I2C0_BASE, 0x01);
I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_START);
while(I2CMasterBusy(I2C0_BASE));
I2CMasterDataPut(I2C0_BASE, 0xF0);
I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH);
while(I2CMasterBusy(I2C0_BASE));
*/
//UART1Write("A\r\n", 3);
//UARTCharPut(UART1_BASE, 'B');
//UART1WriteChar(UARTCharGet(UART0_BASE));
/*
for (i=0; i<8; i++)
{
unsigned char tmp;
i2c_buff[0] = 0x84 | (i << 4);
I2CWrite(0x48, i2c_buff, 1);
if (I2CMasterErr(I2C0_BASE))
printf("Err: %d\r\n", (unsigned int) I2CMasterErr(I2C0_BASE));
I2CRead(0x48, &tmp, 1);
if (I2CMasterErr(I2C0_BASE))
printf("Err: %d\r\n", (unsigned int) I2CMasterErr(I2C0_BASE));
printf("% 3d ", tmp);
}*/
/* toggleRed();
for(i=0; i<=100; i++)
{
setMotor(0, 0.01 * i);
setMotor(1, 0.01 * i);
setMotor(2, 0.01 * i);
setMotor(3, 0.01 * i);
printf("%d\r\n", i);
SysCtlDelay(SysCtlClockGet() / 3 / 100);
}
toggleRed();
for(; i>0; i--)
{
setMotor(0, 0.01 * i);
setMotor(1, 0.01 * i);
setMotor(2, 0.01 * i);
setMotor(3, 0.01 * i);
printf("%d\r\n", i);
SysCtlDelay(SysCtlClockGet() / 3 / 100);
}*/
//printf("%d\r\n", (unsigned int) (((ADCRead(3) >> 4) - 45) * 0.45));
//setServo(0, (((ADCRead(3) >> 4) - 45) * 0.45) / 100.0);
/*
I2CMasterSlaveAddrSet(I2C0_BASE, 0x18, false); // Set Outputs Directions
I2CMasterDataPut(I2C0_BASE, 0x01);
I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_START);
while(I2CMasterBusy(I2C0_BASE));
if (GPIOPinRead(GPIO_PORTF_BASE, GPIO_PIN_4) == 0)
I2CMasterDataPut(I2C0_BASE, 0x80);
else
I2CMasterDataPut(I2C0_BASE, 0x70);
I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH);
while(I2CMasterBusy(I2C0_BASE));*/
/*I2CMasterSlaveAddrSet(I2C0_BASE, 0x18, false);
I2CMasterDataPut(I2C0_BASE, 0x00);
I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_SINGLE_SEND);
while(I2CMasterBusy(I2C0_BASE));
I2CMasterSlaveAddrSet(I2C0_BASE, 0x18, true);
I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_SINGLE_RECEIVE);
while(I2CMasterBusy(I2C0_BASE));
if( (I2CMasterDataGet(I2C0_BASE) & 0x01) != 0)
toggleBlue();*/
}
}
void hardwareInit(){
// Configure the device for maximum performance but do not change the PBDIV
// Given the options, this function will change the flash wait states, RAM
// wait state and enable prefetch cache but will not change the PBDIV.
// The PBDIV value is already set via the pragma FPBDIV option above..
SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
SYSTEMConfigPerformance(80000000);
(_TRISF5)=INPUT; // for the reset sw
ATX_DISENABLE();
CloseTimer2();
Pic32_Bowler_HAL_Init();
Bowler_Init();
clearPrint();
println_I("\n\n\nStarting PIC initialization");
FlashGetMac(MyMAC.v);
DelayMs(2000);//This si to prevent runaway during programming
// enable driven to 3.3v on uart 1
mPORTDOpenDrainClose(BIT_3); mPORTFOpenDrainClose(BIT_5);
char macStr[13];
for (i=0;i<6;i++){
macStr[j++]=GetHighNib(MyMAC.v[i]);
macStr[j++]=GetLowNib(MyMAC.v[i]);
}
macStr[12]=0;
println_I("MAC address is =");
print_I(macStr);
char * dev = "BowlerDevice";
println_I(dev);
//This Method calls INTEnableSystemMultiVectoredInt();
usb_CDC_Serial_Init(dev,macStr,0x04D8,0x0001);
addNamespaceToList((NAMESPACE_LIST *)getBcsCartesianNamespace());
addNamespaceToList((NAMESPACE_LIST *)getBcsPidNamespace());
ATX_ENABLE(); // Turn on ATX Supply, Must be called before talking to the Encoders!!
println_I("Starting Encoders");
initializeEncoders();// Power supply must be turned on first
println_I("Starting Heater");
initializeHeater();
println_I("Starting Servos");
initServos();
#if !defined(NO_PID)
println_I("Starting PID");
initPIDLocal();
#endif
initializeCartesianController();
DelayMs(100);
if( GetPIDCalibrateionState(linkToHWIndex(0))!=CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(1))!=CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(2))!=CALIBRARTION_DONE
){
for(i=0;i<numPidMotors;i++){
SetPIDEnabled(i,true) ;
}
println_I("Running calibration for kinematics axis");
runPidHysterisisCalibration(linkToHWIndex(0));
runPidHysterisisCalibration(linkToHWIndex(1));
runPidHysterisisCalibration(linkToHWIndex(2));
DelayMs(100);//wait for ISR to fire and update all values
for(i=0;i<3;i++){
setPIDConstants(linkToHWIndex(i),.2,.1,0);
}
OnPidConfigure(0);
}else{
println_W("Axis are already calibrated");
}
pid.MsTime=getMs();
//startHomingLinks();
disableSerialComs(true) ;
(_TRISB0)=1;
SetColor(1,1,1);
HEATER_2_TRIS = OUTPUT;
//HEATER_1_TRIS = OUTPUT; // Causes one of the axies to crawl downward in bursts when enabled and on...
//HEATER_0_TRIS = OUTPUT; // causes device to twitc. These are touched by the USB stack somehow..... and as the reset button
}
