//Call this function as often as possible
void updateMotor() {
measureSpeed();
//If the car is tilting too much,shut down the motor and stop the car
if (fabs(getAngle()) > MAX_ANGLE) {
motorOff();
return;
}
speedOut();
}
MotorTestWizardPage::MotorTestWizardPage(QWidget *parent)
: OnOffWizardPage(parent),
m_timer(new QTimer(this)),
m_theta(0.0)
{
memset(m_on, false, sizeof_array(m_on) * sizeof(bool));
setTitle(tr("Motor Test"));
connect(this, SIGNAL(on(quint16)), SLOT(motorOn(quint16)));
connect(this, SIGNAL(off(quint16)), SLOT(motorOff(quint16)));
}
void motorReset(void) {
//motorReverse(); Pending tests
//loop_until_bit_is_clear(PINA,PA3)
while(PINB & _BV(PB1)) {
motorReverse();
}
motorOff();
systemReset = TRUE;
currentPosition = 0;
//sysTransmitString(terminalUartIndex, "Height Reset\n");
}
void motorDown(char* distance) {
mm = atoi(distance);
readingsTaken = 0;
readingsWanted = mm * encoderCount; //Each mm of wire correlates to about 9.449 readings
if(currentPosition <511){
motorForward();
_delay_ms(20);
estimateCounter = 0;
estimatedPosition = currentPosition;
while(readingsTaken < readingsWanted) {
if(estimatedPosition > 584){
//sysTransmitString(terminalUartIndex, "At minimum height.\n");
motorOff();
break;
}
//Checks for low signal, then high signal, then increments readingTaken
while(PINA & _BV(PA6)){}
while(!(PINA & _BV(PA6))){}
readingsTaken++;
estimateCounter++;
//Increments estimated counter every 9 readings taken
if(estimateCounter == 4){
estimatedPosition++;
estimateCounter = 0;
}
}
//sysTransmitString(terminalUartIndex, "Motor Done.\n");
} else{
//sysTransmitString(terminalUartIndex, "At minimum height.\n");
}
//Changes the systems idea of where the light hood is
//Only changes if the system has been zeroed already
if(systemReset == TRUE){
currentPosition = currentPosition + (readingsTaken/encoderCount);
}
motorOff();
}
/*main function to run the filter wheel from the command line */
int main(int argc, char *argv[])
{
memset(out,0,64);
memset(in,0,64);
int c;
setup(); //start the communications
while ((c = getopt (argc, argv, "pmftoiczh")) != -1)
switch (c)
{
if(argc==1){
case 'p':
currentPos();
break;
case 't':
printf("test\n");
break;
case 'o':
motorOff();
break;
case 'z':
zero();
break;
case '?':
if (isprint (optopt))
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
else
fprintf (stderr,
"Unknown option character `\\x%x'.\n",
optopt);
return 1;
default:
abort ();
}
if(argc==2){
case 'm':
printf("%s\n", argv[2]);
moveMotor(argv[2]);
break;
case 'f':
printf("%s\n", argv[2]);
moveToFilter(atoi(argv[2]), atoi(argv[3]));
break;
}
}
return 1;
}
void setMotorPWM( uint16_t match , uint16_t top ) {
if (match==0) { // Special case this because the PWM generator still generates a pulse at 0 duty cycle
// "If the OCR1x is set equal to BOTTOM (0x0000) the output will be a narrow spike for each TOP+1 timer clock cycle."
motorOff();
} else {
// Set OC1B on Compare Match
// Clear OC1B at BOTTOM (inverting mode)
// Fast PWM, TOP= OCR1A, Update OCR1x at top
// Clock select clk I/O/1 (No prescaling)
// Assign TOP first to make sure we don't miss the match
OCR1A = top; // Set TOP. Freq should be IOclk/OCR1A = 16Khz
OCR1B = match; // Set match which sets duty cycle
// 0bxx100000 COM1B PWM Fast mode, Clear OC1A/OC1B on Compare Match, set OC1A/OC1B at BOTTOM (non-inverting mode)
// 0bxxxxxx11 WGM[11:10] Fast PWM, TOP=OCR1A, Update at OCR TOP
TCCR1A = 0b00100011;
// 0b00011000 WGM[13:12] Fast PWM TOP=OCR1A UPDATE=TOP, Compare output on pin
// 0b00000001 CS clk I/O/1 (No prescaling)
TCCR1B = 0b00011001;
// "The actual OC1x value will only be visible on the port pin if the data direction for the port pin is set as output (DDR_OC1x)."
// We set to output mode on startup
}
}
void motorUp(char* distance) {
mm = atoi(distance);
readingsTaken = 0;
readingsWanted = mm * encoderCount;
//Ensures bump sensor isn't active
if(PINB & _BV(PB1)){
motorReverse();
_delay_ms(20);
while(readingsTaken < readingsWanted) {
//Checks each loop to see if bump sensor hit
if(PINB & _BV(PB1)){
//Checks encoder for high signal
while(PINA & _BV(PA6)){
if(!(PINB & _BV(PB1))){
//Bump sensor hit, turn off motor and end loop
//sysTransmitString(terminalUartIndex, "Roof hit.\n");
motorOff();
systemReset = TRUE;
currentPosition = 0;
return;
}
}
//Checks encoder for low signal
while(!(PINA & _BV(PA6))){
if(!(PINB & _BV(PB1))){
//Bump sensor hit, turn off motor and end loop
//sysTransmitString(terminalUartIndex, "Roof hit.\n");
motorOff();
systemReset = TRUE;
currentPosition = 0;
return;
}
}
readingsTaken++;
} else{
//Bump sensor hit, turn off motor and end loop
//sysTransmitString(terminalUartIndex, "Roof hit - ");
motorOff();
systemReset = TRUE;
currentPosition = 0;
return;
}
}
//Changes the systems idea of where the light hood is
//Only changes if the system has been zeroed already
if(systemReset == TRUE){
currentPosition = currentPosition - (readingsTaken/encoderCount);
}
//sysTransmitString(terminalUartIndex, "Motor Done.\n");
}
else{
//sysTransmitString(terminalUartIndex, "At roof.\n");
systemReset = TRUE;
currentPosition = 0;
}
motorOff();
}
int main(void)
{
motorInit(); // Initialize the motor port to drive the MOSFET low
uint8_t watchDogResetFlag = MCUSR & _BV(WDRF); /// Save the watchdog flag
MCUSR &= ~ _BV( WDRF ); // Clear the watchdog flag
// "In safety level 1, WDE is overridden by WDRF in MCUSR...."
// "This means that WDE is always set when WDRF is set."
// IF we left this Set, then we could get watchdogged while sleeping
wdt_enable( WDTO_8S ); // Give ourselves 8 seconds before forced reboot
enableTimer0(); // Initialize the timer that also PWMs the LEDs
WHITE_LED_DDR |= _BV(WHITE_LED_BIT); // Pin to output
RED_LED_DDR |= _BV(RED_LED_BIT);
// Button sense pin setup
BUTTON_PORT |= _BV(BUTTON_BIT); // Enable pull-up for button pin
// Battery Charger status pin setup
EOC_PORT |= _BV(EOC_BIT); // Activate pull-up
CIP_PORT |= _BV( CIP_BIT); // Activate pull-up
_delay_us(1); // Give the pull-ups a second to work
if ( !watchDogResetFlag ) { // Are we coming out of anything except for a WatchDog reset?
// Cold boot, run test mode
// Blink back and forth to show LEDs work and solicit a button press
for(uint8_t i=0;i<100 && !BUTTON_STATE_DOWN(); i++ ) {
setRedLED(255);
for(uint8_t j=0; j<100 && !BUTTON_STATE_DOWN();j++ ) {
_delay_ms(1);
}
setRedLED(0);
setWhiteLED(255);
for(uint8_t j=0; j<100 && !BUTTON_STATE_DOWN();j++ ) {
_delay_ms(1);
}
setWhiteLED(0);
wdt_reset();
}
_delay_ms(BUTTON_DEBOUNCE_TIME_MS);
// TODO: Put more code here for some testing and feedback on initial battery connection at the factory.
}
// Ready to begin normal operation!
if (BUTTON_STATE_DOWN()) { // Possible stuck button?
// If we get here, either we just finished test mode and the button is Still down, in which case
// we are testing to make sure it goes back up
// Otherwise we just reset and the button was down when we woke, so likely it is stuck down
// and that is what caused the reset. In this case, show the user and then eventually disable the button.
// Each pass of this loop takes ~1 sec.
for( uint16_t t=0; (t <= BUTTON_TRANSIT_TIMEOUT_S) && BUTTON_STATE_DOWN(); t++ ) {
// To indicate that we are in a stuck-button sequence, we will blink the white LED
// at 50% brightness, 1Hz, 10% duty cycle. This looks different than other states
// and also minimizes battery usage (the LED pulls 10+mA) since we might be doing this
// for many minutes
// Start with LED off because it looks better when we are coming in from a watchdog
// reset because the button was held down for more than 8 secs. Otherwise user
// sees an odd blink pattern.
setWhiteLED(0);
// Leave the white LED off for 900 ms or until the button goes up
for( uint8_t l=0; l<90 && BUTTON_STATE_DOWN() ; l++) {
_delay_ms(10);
}
setWhiteLED(BUTTON_FEEDBACK_BRIGHTNESS);
// Leave the white LED on for 100 ms or until the button goes up
// Could do this as a single _delay_ms(100) but that might feel un-responsive
for( uint8_t l=0; l<10 && BUTTON_STATE_DOWN() ; l++) {
_delay_ms(10);
}
wdt_reset();
}
// Turn off LED before continuing
setWhiteLED(0);
// Debounce the possible button up transition
_delay_ms(BUTTON_DEBOUNCE_TIME_MS);
}
if (BUTTON_STATE_DOWN()) { // Do we still have a stuck button?
// Indicate we are entering transit mode with a quick double flash of both LEDs
setRedLED(255);
setWhiteLED(255);
_delay_ms(100);
setRedLED(0);
setWhiteLED(0);
_delay_ms(100);
setRedLED(255);
setWhiteLED(255);
_delay_ms(100);
setRedLED(0);
setWhiteLED(0);
BUTTON_PORT &= ~_BV(BUTTON_BIT); // Disable pull up to avoid running the battery down.
// Do not enable interrupt on button pin change - we will require a charger state change to wake up
// Since the interrupt is not enabled, the pin will be disconnected during sleep so any floating
// on it will not waste power.
} else {
// Leave pull-up enabled
PCMSK1 = _BV(BUTTON_INT); // Enable interrupt on button pin so we wake on a press
}
PCMSK0 = _BV(EOC_INT) | _BV(CIP_INT); // Enable interrupt on change in state-of-charge pin or end-of-charge pin no matter what
GIMSK |= _BV(PCIE1) | _BV(PCIE0); // Enable both pin change interrupt vectors (each individual pin was also be enabled above)
// Clear pending interrupt flags. This way we will only get an interrupt if something changes
// after we read it. There is a race condition where something could change between the flag clear and the
// reads below, so code should be able to deal with possible redundant interrupt and worst case
// is that we get woken up an extra time and go back to sleep.
GIFR = _BV(PCIF1) | _BV(PCIF0); // Clear interrupt flags so we will interrupt on any change after now...
if ( !CIP_STATE_ACTIVE() && !EOC_STATE_ACTIVE() ) { // Check if conditions are ALREADY true since we only wake on change....
// Ok, it is bedtime!
set_sleep_mode( SLEEP_MODE_PWR_DOWN ); // Go into deep sleep where only a pin change can wake us.. uses only ~0.1uA!
// GOOD NIGHT!
// This code disables the Watchdog. Note that we can not use the library wdt_disable() becuase it has a bug
// that causes intermittent unwanted resets.
// Note interrupts are already clear when we get here, otherwise we would need to worry about getting interrupted between the two following lines
WDTCSR |= _BV(WDCE) | _BV(WDE); // In the same operation, write a logic one to WDCE and WDE.
// Note we use OR to preserve the prescaler
WDTCSR = 0; // Within the next four clock cycles, in the same operation, write the WDE and WDP bits
// as desired, but with the WDCE bit cleared.
sleep_enable(); // "To enter any of the three sleep modes, the SE bit in MCUCR must be written to logic one and a SLEEP instruction must be executed."
sei(); // Enable global interrupts. "When using the SEI instruction to enable interrupts, the instruction following SEI will be executed before any pending interrupts."
sleep_cpu(); // This must come right after the sei() to avoid race condition
// GOOD MORNING!
// If we get here, then a button push or change in charger status woke s up....
sleep_disable(); // "To avoid the MCU entering the sleep mode unless it is the programmer’s purpose, it is recommended to write the Sleep Enable (SE) bit to one just before the execution of the SLEEP instruction and to clear it immediately after waking up."
cli(); // We are awake now, and do don't care about interrupts anymore (out interrupt routines don't do anything anyway)
wdt_enable( WDTO_8S ); // Re-enable watchdog on wake Give ourselves 8 seconds before reboot
}
// Ok, now we are running!!!
// Motor speed
uint8_t currentSpeedStep = 0; // What motor speed setting are we currently on?
while (1) {
// This main loop runs for as long as the motor is on.
// It can be terminated by battery charger change of state, low battery detection, button press back to 0 speed, or long button press
// All these changes terminate the loop in a reboot.
if (EOC_STATE_ACTIVE()) { // End of charge?
motorOff(); //Turn motor off in case were running before plug went in
setWhiteLED(255); // White LED full on
_delay_ms( JACK_DEBOUNCE_TIME_MS );
while (EOC_STATE_ACTIVE()); // White LED on for as long as we are charging....
// Note that this will watchdog timeout after 8 seconds and reboot us,
// After which we will immediately fall right back to here and continue to show the white LED
setWhiteLED(0); // Turn it off now, for instant feedback if unplugged (otherwise it will be on for extra 250ms waiting for watchdog reset)
// Charger unplugged, reboot for goo measure
REBOOT();
}
if (CIP_STATE_ACTIVE()) { // Charging?
motorOff(); //Turn motor off in case were running before plug went in
uint8_t brightness=0;
int8_t direction=1;
_delay_ms( JACK_DEBOUNCE_TIME_MS );
while (CIP_STATE_ACTIVE()) { // White LED pulse for as long as we are charging....
setWhiteLED(brightness);
if (brightness==255) {
direction=-1;
} else if (brightness==0) {
direction=1;
}
brightness+=direction;
_delay_ms(1); // Slows the speed of the rampping LED
wdt_reset();
}
setWhiteLED(0); // Turn it off now, for instant feedback if unplugged (otherwise it will be on for extra 250ms waiting for watchdog reset)
// All done charing, reboot for good measure
REBOOT();
}
uint8_t vccx10 = readVccVoltage(); // Capture the current power supply voltage. This takes ~1ms and will be needed multiple times below
if ( vccx10 <= LOW_BATTERY_VOLTS_COLDx10) {
if ( (currentSpeedStep==0) || ( vccx10 <= LOW_BATTERY_VOLTS_WARMx10) ) { // Motor off, or running and really low?
motorOff();
setWhiteLED(0); // Needed becuase both LEDs might be on if we are in the middle of a button press
setRedLED(255);
_delay_ms(LOW_BATTERY_LED_ONTIME_MS); // Show red LED to user to show low battery
while (BUTTON_STATE_DOWN()); // Wait for button to be released if pressed
// Will watchdog timeout in 8 seconds if stuff
setRedLED(0);
REBOOT();
}
}
uint8_t buttonPressedFlag=0;
if (BUTTON_STATE_DOWN()) { // Button pushed?
setWhiteLED(BUTTON_FEEDBACK_BRIGHTNESS);
_delay_ms(BUTTON_DEBOUNCE_TIME_MS); // debounce going down...
if ( currentSpeedStep ==0 ) { // Special case first press turning on instantly
updateMotor( pgm_read_word(&speedSteps[1].top) , pgm_read_word(&speedSteps[1].normailzedDuty), vccx10); // Set new motor speed
}
uint16_t buttonDownCount=0;
while (BUTTON_STATE_DOWN()) { // Wait for button to go back up or longpress timeout
if (buttonDownCount++ >= BUTTON_LONG_PRESS_MS ) { // Long press? Shut motor off
// The reboot would do both of these anyway, but we do them redundantly here so UI feels responsive-
// The full reboot cycle takes 100+ ms.
motorOff();
setWhiteLED(0);
REBOOT();
// If the button is still down once we reboot, we will land in the stuck button detection sequence
// which will blink the LED for 1/10th second every second until either the button goes up
// or the transit mode button timeout expires
}
_delay_ms(1); // One loop=~1ms
}
// Pressed less than a long press
buttonPressedFlag=1; // Debounce after setting new motor speed so UI feels responsive
currentSpeedStep++;
if (currentSpeedStep >= SPEED_STEP_COUNT ) { // Cycled all the Way around?
currentSpeedStep=0;
}
}
updateMotor( pgm_read_word(&speedSteps[currentSpeedStep].top) , pgm_read_word(&speedSteps[currentSpeedStep].normailzedDuty), vccx10); // Set new motor speed
if (buttonPressedFlag) {
// Button released, white LED off again
setWhiteLED(0);
_delay_ms(BUTTON_DEBOUNCE_TIME_MS); // debounce the button returning back up
}
if (currentSpeedStep==0) { // Either we stepped though the settings back to off, or we got a spurious wake up
REBOOT();
}
// If we get to here, then we check for a low battery and had the chance to reboot if we found one,
// so ok to postpone reset...
wdt_reset();
}
}
