int16_t main(void) { unsigned int buttonCounter = 20000; int i; /* Configure the oscillator for the device */ ConfigureOscillator(); /* Initialize IO ports and peripherals */ InitApp(); //initSerial(); initSPI1(); initSPI2(); InitI2C(); __delay32(1600000); // allow for POR for all devices initnRF(); ControlByte = 0x00D0; // mpu6050 address InitMPU6050(ControlByte); // InitHMC5883L(); __delay_ms(500); /** Init control loop *****************************************************/ readSensorData(); accXangle = (atan2(accel[0], accel[2])*RAD_TO_DEG); accYangle = (atan2(accel[1], accel[2])*RAD_TO_DEG); gyroXangle = accXangle; gyroYangle = accYangle; compAngleX = accXangle; compAngleY = accYangle; AngleOffset[0] = accXangle; AngleOffset[1] = accYangle; CalibrateGyro(); // Finding the gyro zero-offset SetupInterrupts(); ////// int serStringN = 14; ////// char nRFstatus = 0; //////// int i; ////// delaytime = 1; ////// bool mode = 0; while(1) { /** Read Button RB7 for enable steppers *******************************/ if (buttonState == 0 && PORTBbits.RB7 && !buttonCounter) { buttonState = 1; buttonCounter = 20000; enableSteppers = 0; } if (buttonState == 1 && !PORTBbits.RB7 && !buttonCounter) { enableSteppers = 2; __delay32(40000000); buttonState = 2; buttonCounter = 20000; enableSteppers = 1; } if (buttonState == 2 && PORTBbits.RB7 && !buttonCounter) { buttonState = 3; buttonCounter = 20000; enableSteppers = 0; } if (buttonState == 3 && !PORTBbits.RB7 && !buttonCounter) { buttonState = 0; buttonCounter = 20000; enableSteppers = 0; } if (buttonCounter) { buttonCounter--; } for (i=0;i<100;i++) { i=i; } // __delay32(100); } }
int main(void) { int pwm; int adc_on; int adc_off; uint16_t i; element elem; robot.rotation_target = 0; robot.right_motor_pos =0; robot.left_motor_pos =0; robot.right_motor_target =0; robot.left_motor_target =0; SysTick_Config(168000000/8/1000000); // interrupr 1000000 per secound initTimer3(1); initMotors(); initEncoders(); initUsart3(); // enableUSART3RXNEInterrupt(); initADC(); initIRSensors(); initLED(); initSPI2(); LED_OFF; waitMs(1000); LED_ON; initPIDStructures(); stopMotors(); resetEncoders(); updateRobotState(); robot.left_ir_sensor_target = measures.left_ir_sensor; robot.right_ir_sensor_target = measures.right_ir_sensor; initQueue(&robot_queue); //addMove(&robot_queue,FORWARD,2000); addMove(&robot_queue,ROTATE,20000); // addMove(&robot_queue,FORWARD,9000); // addMove(&robot_queue,ROTATE,0); // addMove(&robot_queue,ROTATE,6000); // addMove(&robot_queue,FORWARD,9000); // addMove(&robot_queue,ROTATE,6000); // addMove(&robot_queue,ROTATE,12000); // addMove(&robot_queue,FORWARD,9000); // addMove(&robot_queue,ROTATE,12000); // addMove(&robot_queue,ROTATE,3000); // nextMove(); LED_ON; while(1) { if ( isBatteryWeak() ){ stopMotors(); blinkLed(); } if (flags.update_robot == 1){ moveRobot(); updateRobotState(); flags.update_robot =0; } if (flags.usart_custom == 1){ USART3_transmitString(itoa((int) robot.rotation, buf,10)); USART3_transmitString(" "); USART3_transmitString(itoa((int) robot.rotation_target, buf,10)); USART3_transmitString("\n"); } } // flags.usart_custom == 0; // USART3_transmitString("lewe_kolo_diff "); // USART3_transmitString(itoa((int) robot.left_motor_target - robot.left_motor_pos , buf,10)); // USART3_transmitByte('\n'); // USART3_transmitString("lewe kolo pwm "); // USART3_transmitString(itoa((int) getTranslation(LEFT_MOTOR) , buf,10)); // USART3_transmitByte('\n'); // // USART3_transmitString("prawe kolo diff "); // USART3_transmitString(itoa((int) robot.right_motor_target - robot.right_motor_pos , buf,10)); // USART3_transmitByte('\n'); // USART3_transmitString("prawe kolo pwm "); // USART3_transmitString(itoa((int) getTranslation(RIGHT_MOTOR) , buf,10)); // USART3_transmitByte('\n'); // } // // if (flags.usart_graph == 1){ // static uint8_t usart_start = 1; // if (usart_start ){ // usart_start=0; // USART3_transmitString("\n"); // USART3_transmitString("\n"); // USART3_transmitString("\n"); // USART3_transmitString("__start__\n"); // USART3_transmitString("lewe_kolo_pozycja "); // USART3_transmitString("lewe_kolo_cel "); // USART3_transmitString("lewe_kolo_diff "); // USART3_transmitString("lewe_kolo_pwm_T "); // USART3_transmitString("prawe_kolo_pozycja "); // USART3_transmitString("prawe_kolo_cel "); // USART3_transmitString("prawe_kolo_diff "); // USART3_transmitString("prawe_kolo_pwm_T \n"); // USART3_transmitString(" \n"); // } // // USART3_transmitString(itoa((int) robot.right_motor_pos , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) robot.right_motor_target , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) robot.right_motor_target - robot.right_motor_pos , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) getTranslation(RIGHT_MOTOR) , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) robot.left_motor_pos , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) robot.left_motor_target , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) robot.left_motor_target - robot.left_motor_pos , buf,10)); // USART3_transmitString(" "); // USART3_transmitString(itoa((int) getTranslation(LEFT_MOTOR) , buf,10)); // USART3_transmitByte('\n'); // flags.usart_graph = 0; // } // } // return 0; }