void Motor_Init(){
#ifdef MOTOR1A_ANSEL
  MOTOR1A_ANSEL = 0;
#endif
#ifdef MOTOR2A_ANSEL
  MOTOR2A_ANSEL = 0;
#endif
#ifdef MOTOR1B_ANSEL
  MOTOR1B_ANSEL = 0;
#endif
#ifdef MOTOR2B_ANSEL
  MOTOR2B_ANSEL = 0;
#endif
  Motor_Disable();
  Motor_Set1Forward();
  Motor_Set2Forward();
  Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
  Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
  TMR2 = 0; // Set Timer 2 to Zero
  PR2 = 10230; // Set Period
  T2CONbits.TCKPS = 0; // Set Prescale of 1
  OC2CONbits.OCM = 6; // PWM Mode on OC, Fault Pin Disabled
  OC4CONbits.OCM = 6; // PWM Mode on OC, Fault Pin Disabled
  OC2CONbits.OCTSEL = 0; // Use Timer 2
  OC4CONbits.OCTSEL = 0; // Use Timer 2
  T2CONbits.ON = 1; // Enable Timer 2
  OC2CONbits.ON = 1; // Enable Output Compare 2
  OC4CONbits.ON = 1; // Enable Output Compare 4
}
Exemple #2
0
void motorTest(SmartCar * smartCar) {
	int16_t speed = 300;

	Motor_Enable(&smartCar->motor);
	Servo_init(&smartCar->servo);
	Servo_runAs(&smartCar->servo,0);
	
	while (1) {
		Servo_runAs(&smartCar->servo,0);
			
		smartCar->motor.sendPID = 1;//for bluetooth send start
		Motor_runAs(&smartCar->motor, speed);

		Segment_print(&smartCar->segment[0], smartCar->motor.targetSpeed);
		Segment_print(&smartCar->segment[1], Encoder_get(&smartCar->encoder));
		Segment_print(&smartCar->segment[2], smartCar->motor.currentSpeed);
		
		switch (board.button.check()) {
		case 1: // fast
			if (speed < 2000) {
				speed = speed + 50;
			} else {
				speed = 2000;
			}
			break;

		case 2: // slow
			if (speed > -2000) {
				speed = speed - 50;
			} else {
				speed = -2000;
			}
			break;

		case 3: // reverse
			speed = -speed;
			break;

		case 4: //motor test end
			smartCar->motor.sendPID = 0;//for bluetooth send stop
			speed = 0;
			Motor_Disable(&smartCar->motor);
			Segment_print(&smartCar->segment[0], speed);
			Segment_print(&smartCar->segment[1], speed);
			return;
		}
	}
}
void main(void){
  SYSTEMConfigPerformance(10000000);
  STATE state = START;
  unsigned short int lineCount = 0;
  bool ignoreTurns = false;
  // Initialize Motor(s))
  Motor_Init();
  // Initialize IRSensor(s)
  IRSensor_Init();
  // LCD Initialize
  LCD_Init();
  while(1){
    printDebug(state);
    switch(state){
      case START:
        Motor_Disable();
        if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL))state = TRAVEL;
        break;
      case TRAVEL:
        Motor_Enable();
        if(IRSensor_CheckLeftFront(200)){
          state = TURN_LEFT;break;
        }else if(IRSensor_CheckRightFront(200)){
          state = TURN_RIGHT;break;
        }else if(IRSensor_GetCenterLeft() < 300 || IRSensor_GetCenterRight < 300){
          if(IRSensor_GetCenterLeft() > IRSensor_GetCenterRight()){
            Motor_Set1DutyCycle(0);
            Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
          }else{
            Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
            Motor_Set2DutyCycle(0);
          };
        }else if(IRSensor_GetCenterLeft() != IRSensor_GetCenterRight()){
          float biasValue = 1.0 + abs(IRSensor_GetCenterLeft() - IRSensor_GetCenterRight()) / 2000.0;
          if(IRSensor_GetCenterLeft() > IRSensor_GetCenterRight()){
            Motor_Set1DutyCycle(biasValue * STANDARD_DUTY_CYCLE);
            Motor_Set2DutyCycle(1.0/biasValue * STANDARD_DUTY_CYCLE);
          }else{
            Motor_Set1DutyCycle(1.0/biasValue * STANDARD_DUTY_CYCLE);
            Motor_Set2DutyCycle(biasValue * STANDARD_DUTY_CYCLE);
          };
        }else{
          Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
          Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
        };
        break;
      case TURN_LEFT:
        if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
          Motor_Disable();
          state = TRAVEL;
        }else{
          Motor_Set1DutyCycle(0.0);
          Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE * MOTOR_BIAS_TURN);
          Motor_Enable();
        };
        break;
      case TURN_RIGHT:
        if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
          Motor_Disable();
          state = TRAVEL;
        }else{
          Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE * MOTOR_BIAS_TURN);
          Motor_Set2DutyCycle(0.0);
          Motor_Enable();
        };
        break;
    };
  };
};
void oldmain(void){
  SYSTEMConfigPerformance(10000000);
  STATE state = START;
  unsigned short int lineCount = 0;
  bool ignoreTurns = false;
  // Initialize Motor(s))
  Motor_Init();
  // Initialize IRSensor(s)
  IRSensor_Init();
  // LCD Initialize
  LCD_Init();
  Motor_Enable();
  while(1){
    printDebug(state);
    switch(state){
      case START:
        Motor_Disable();
        if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL))state = TRAVEL;
        break;
      case TURN_LEFT:
        if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
          Motor_Disable();
          state = TRAVEL;
        }else{
          Motor_Set1DutyCycle(0.0);
          Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE * MOTOR_BIAS_TURN);
          Motor_Enable();
        };
        break;
      case TURN_RIGHT:
        if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
          Motor_Disable();
          state = TRAVEL;
        }else{
          Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE * MOTOR_BIAS_TURN);
          Motor_Set2DutyCycle(0.0);
          Motor_Enable();
        };
        break;
      case TRAVEL:
        Motor_Set1Forward();
        Motor_Set2Forward();
        Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
        Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
        Motor_Enable();
        if(!IRSensor_CheckCenterRight(IR_TRIGGER_ADJUST) && IRSensor_CheckCenterLeft(IR_TRIGGER_ADJUST)){
          Motor_Disable();
          state = ADJUST_RIGHT;break;
        }else if(!IRSensor_CheckCenterLeft(IR_TRIGGER_ADJUST) && IRSensor_CheckCenterRight(IR_TRIGGER_ADJUST)){
          Motor_Disable();
          state = ADJUST_LEFT;break;};
        //  if(IRSensor_CheckFront()){
        //  lineCount++;
        //  if(lineCount == 1){// Entering Extra Credit 'T' Intersection
        //    state = TURN_LEFT;
        //  }else if(lineCount == 2){// Exiting Extra Credit 'T' Intersection
        //    state = TURN_RIGHT;
        //  }else if(lineCount == 5){// End of First Time Through
        //    state = TURN_AROUND;
        //  };
        //};
        if(!ignoreTurns && IRSensor_CheckLeftFront(IR_TRIGGER_TURN)){Motor_Disable();state = TURN_LEFT;break;};
        if(!ignoreTurns && IRSensor_CheckRightFront(IR_TRIGGER_TURN)){Motor_Disable();state = TURN_RIGHT;break;};//
        break;
      case ADJUST_LEFT:
        Motor_Set1DutyCycle(MOTOR_BIAS_ADJUST * STANDARD_DUTY_CYCLE);
        Motor_Set2DutyCycle(0.75 * MOTOR_BIAS_ADJUST * STANDARD_DUTY_CYCLE);
        Motor_Enable();
        if(IRSensor_CheckLeftFront(IR_TRIGGER_TURN)){
          state = TURN_LEFT;
          break;
        }else if(IRSensor_CheckRightFront(IR_TRIGGER_TURN)){
          state = TURN_RIGHT;
          break;
        }else if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
          Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
          Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
          state = TRAVEL;
          break;
        }else{
          break;
        };
      case ADJUST_RIGHT:
        Motor_Set1DutyCycle(0.75 * MOTOR_BIAS_ADJUST * STANDARD_DUTY_CYCLE);
        Motor_Set2DutyCycle(MOTOR_BIAS_ADJUST * STANDARD_DUTY_CYCLE);
        Motor_Enable();
        if(IRSensor_CheckLeftFront(IR_TRIGGER_TURN)){
          state = TURN_LEFT;
          break;
        }else if(IRSensor_CheckRightFront(IR_TRIGGER_TURN)){
          state = TURN_RIGHT;
          break;
        }else if(IRSensor_CheckCenter(IR_TRIGGER_TRAVEL)){
          Motor_Set1DutyCycle(STANDARD_DUTY_CYCLE);
          Motor_Set2DutyCycle(STANDARD_DUTY_CYCLE);
          state = TRAVEL;
          break;
        }else{
          break;
        };
      //case TURN_AROUND:
      //  while(IRSensor_CheckCenterLeft(IR_TRIGGER_TRAVEL)){
      //    Motor_Set1Backward();
      //    Motor_Set2Forward();
      //  };
      //  delayMs(IR_DELAY_TURNAROUND);
      //  state = TRAVEL;
      //  ignoreTurns = true;
      //  break;
      //case END:
      //  Motor_Disable();
      //  break;
    };
  };
};