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 }