void main (void)
{
//Set Clock Frequency = 500kHz
OSCCONbits.IRCF2 = 0;
OSCCONbits.IRCF2 = 1;
OSCCONbits.IRCF2 = 1;
// Enable Global interrupts
INTCONbits.GIE = 1; // Enable High priority interrupt
//Setup IR Sensor
setupIRSensor();
//Initializes the timer2 for PWM for Motor and Servo
setTimer2PWM();
//Setup Motor
setupMotor();
//Setup Timer for servo
startTimer0();
//Setup Timer for timed water
startTimer1();
//SetupButtonInterupt
setupButtons();
while (1)
{
// This area loops forever
}
}
// +++++++++++++++++++++++++ main loop +++++++++++++++++++++++++++++++++++++++++++
void setup()
{
// init
nh.initNode();
// setup
setupMotor();
setupSensor();
setupServo();
// advertise
nh.advertise(pub_sensor_tracks);
nh.advertise(pub_range);
// subscribe
nh.subscribe(subscriberServo);
nh.subscribe(motor_sub);
}
int main(void)
{
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
// setup the pins and timers for the motor control
setupMotor();
//initialze the SPI
setup_IMU_SPI();
int z_gyro_data;
while(1)
{
// grab IMU data
z_gyro_data = read_IMU_SPI(ZGYRO);
//store in an array to plot later
data_array[i] = z_gyro_data;
i++;
// if CW rotation is detected, make a PWM that is linearly proportional to the IMU data
if(z_gyro_data >= 30){
motorSpeed(z_gyro_data);
//spin the motor in CW direction
motorCW();
motorON();
}
// if CCW motion is detected, make a PWM that is linearly proportional to the IMU data
if(z_gyro_data < -30){
// invert the magnitude, so a positive value can be sent to PWM
z_gyro_data = z_gyro_data * -1;
motorSpeed(z_gyro_data);
// spin motor CCW
motorCCW();
motorON();
}
else{
motorOFF();
}
//__delay_cycles(2000);
}
}
void setupMotors() {
wiringPiSetup();
setupMotor(motor1);
setupMotor(motor2);
}
