int main(void) { int rc = gpioInitialise(); if (rc < 0) { printf("gpioInitialise() failed: %d\n", rc); exit(-1); } int pwmPin = 17; int servoSweepDuration = 500; // Time in ms printf("pwmPin pin = %d\n", pwmPin); printf("servoSweepDuration = %d seconds\n", servoSweepDuration); int direction = 0; // 0 = up, 1 = down while(1) { unsigned int startTime = gpioTick(); unsigned int sweepTime = servoSweepDuration * 1000 / 2; unsigned int delta = gpioTick() - startTime; while(delta < sweepTime) { // Position is going to be between 0 and 1 and represents how far along the sweep // we are. double position = (double)delta/sweepTime; unsigned int timeToWriteHighUSecs; position = -0.4; if (direction == 0) { timeToWriteHighUSecs = (position + 1.0) * 1000; } else { timeToWriteHighUSecs = (2.0 - position) * 1000; } printf("Setting pulse width %d, %d, %d\n", timeToWriteHighUSecs, delta, startTime); gpioServo(pwmPin, timeToWriteHighUSecs); gpioDelay(20 * 1000); delta = gpioTick() - startTime; } // End of a sweep in one direction // Switch direction if (direction == 0) { direction = 1; } else { direction = 0; } } // End of while true return 0; }
void sonarLDRtick(void) { /* trigger a sonar reading */ gpioWrite(SONAR_TRIGGER, PI_ON); usleep(20); gpioWrite(SONAR_TRIGGER, PI_OFF); /* trigger a LDR reading */ gpioSetMode(LDR, PI_OUTPUT); /* drain capacitor */ gpioWrite(LDR, PI_OFF); usleep(200); LDRrechargeTick = gpioTick(); gpioSetMode(LDR, PI_INPUT); /* start capacitor recharge */ }
void ping(void) { // send out an ultrasonic 'ping' before = 0xffffffff; // set for guard variable gpioSetMode(TRIG, PI_OUTPUT); // trigger a sonar pulse gpioWrite(TRIG, PI_OFF); gpioDelay(5); gpioWrite(TRIG, PI_ON); gpioDelay(10); gpioWrite(TRIG, PI_OFF); gpioDelay(5); gpioSetMode(ECHO, PI_INPUT); before = gpioTick(); // get tick right after sending pulse }
int get_last_AC_zero_crossing() { //static int last_adc_raw_value = 0; //static int min_value = 0; int current_adc_raw_value; static unsigned int count = 0; static int raw_adc_buff[30]; static unsigned int i = 0; static unsigned int tick1 = 0; static unsigned int tick2 = 0; int diffTick; char j; //char ac_present; int sum = 0; //current_adc_raw_value = get_adc_raw(AC_TRANSFORMATOR_SENSOR); raw_adc_buff[i % 30] = current_adc_raw_value; tick2 = gpioTick(); for (j = 0; j < 30; ++j) { sum = sum + raw_adc_buff[j]; //printf(" %.4d", raw_adc_buff[j]); } //printf("\r"); if ((sum / 30) < 400) { ac_present = 0; //no AC i++; } else ac_present = 1; if (ac_present) { diffTick = tick2 - tick1; if ((current_adc_raw_value < 4) && ((diffTick > 21000) || ((diffTick > 9900) && (diffTick < 10100)) || ((diffTick > 19500) && (diffTick < 20500)) ) ) { tick1 = gpioTick(); //zero_tick = tick1; //gpioWrite(PUMP_1_GPIO, 1); // Set PUMP_1_GPIO high. if (diffTick < 10500) { zero_cros_diff_time += diffTick; zero_cros_diff_count ++; } //gpioSetPWMfrequency(18, 100); // Set PUMP_1_GPIO to 100Hz. //gpioPWM(18, 50); //log_to_csv(current_adc_raw_value); //printf("."); //count = i; i++; undetected_zero_crossing_count = 1; return 0; } /* if ((current_adc_raw_value == 0)) //&& (i > count + 12)) { log_to_csv(current_adc_raw_value); printf("."); count = i; i++; //gpioPWM(18, 50); return 0; }*/ /* else if ((raw_adc_buff[(i-2) % 30] > raw_adc_buff[(i-1) % 30]) && (current_adc_raw_value > raw_adc_buff[(i-1) % 30]) && (current_adc_raw_value < 500) && //(i > count + 20) (diffTick > 8000) ) { // //log_to_csv(current_adc_raw_value); // count = i; // if (current_adc_raw_value < 20) gpioPWM(18, 50); tick1 = gpioTick(); zero_tick = tick1; gpioWrite(PUMP_1_GPIO, 1); // Set PUMP_1_GPIO high. //gpioSetPWMfrequency(18, 100); // Set PUMP_1_GPIO to 100Hz. //gpioPWM(18, 50); //log_to_csv(current_adc_raw_value); printf("."); } else {} */ i++; } return 1; }
void start_pwm_pump(void) { unsigned int current_tick; int diffTick; unsigned int requested_pwm1; unsigned int requested_pwm2; //pump_pwm = 2; while (1) { /*pwm_commands_count ++; if (pwm_commands_count > 20000) // ~ 2 second { pwm_commands_count = 0; if (zero_crossing_count > 75) // ~ .75 seconds { ac_present = 1; printf("ac_present = 1\n"); } else { ac_present = 0; printf("ac_present = 0\n"); } zero_crossing_count = 0; } */ if (ac_present == 1) { current_tick = gpioTick(); diffTick = current_tick - zero_tick; //pump 1 if (pump1_pwm > 90) gpioWrite(PUMP_1_GPIO, 0); else if (pump1_pwm < 10) gpioWrite(PUMP_1_GPIO, 1); else { requested_pwm1 = ticks_in_10ms + ZERRO_POINT_OFFSET - (pump1_pwm * 100); if ((diffTick >= requested_pwm1) && (diffTick <= (requested_pwm1 + 1000))) { gpioWrite(PUMP_1_GPIO, 0); } else { gpioWrite(PUMP_1_GPIO, 1); } } //pump 2 if (pump2_pwm > 90) gpioWrite(PUMP_2_GPIO, 0); else if (pump2_pwm < 10) gpioWrite(PUMP_2_GPIO, 1); else { requested_pwm2 = ticks_in_10ms + ZERRO_POINT_OFFSET - (pump2_pwm * 100); if ((diffTick >= requested_pwm2) && (diffTick <= (requested_pwm2 + 1000))) { gpioWrite(PUMP_2_GPIO, 0); } else { gpioWrite(PUMP_2_GPIO, 1); } } } // ac_present = 1 /*else { if (pump1_pwm > 0) { gpioWrite(PUMP_1_GPIO, 0); } else { gpioWrite(PUMP_1_GPIO, 1); } if (pump2_pwm > 0) { gpioWrite(PUMP_2_GPIO, 0); } else { gpioWrite(PUMP_2_GPIO, 1); } }*/ usleep(50); }//while }
/** * Reads in a single set of readings from the sensor * * @param sensorDiff Filled in with the 'decay' time of each sensor */ void readSensor(uint32_t (&sensorDiff)[8]) { uint32_t startTime; // Turn on the IR LEDs gpioWrite(LED_ON, 1); // Configure all the sensors for output for(size_t i = 0; i < 8; i++) { gpioSetMode(sensorGPIOs[i], PI_OUTPUT); gpioWrite(sensorGPIOs[i], 1); } // Wait for the sensors to become 'charged' usleep(20); // Start listening for notifications if(gpioNotifyBegin(notifyPipe, notificationGPIOs) < 0) { std::cerr << "Failed to begin notification" << std::endl; exit(-1); } // Remember our 'start' time startTime = gpioTick(); // And switch all the GPIOs to input for(size_t i = 0; i < 8; i++) { gpioSetMode(sensorGPIOs[i], PI_INPUT); } // wait for the sensors to 'discharge' usleep(1500); // Pause notifications gpioNotifyPause(notifyPipe); // turn off the IR LEDs gpioWrite(LED_ON, 0); // Process results gpioReport_t report; bool dataRead[8] = {0}; // Loop through all the results queued up in the pipe while(sizeof(report) == read(notifyPipeFD, &report, sizeof(report))) { // Not interested in any of the flags (e.g. keep alive) so we // skip them if(report.flags) { continue; } // We need to detect when a GPIO has from high to low, // so for each message walk through all the sensors to // see if they have changed for(size_t i = 0; i < 8; i++) { // Has the sensor become 'low' for the first time if(!dataRead[i] && 0 == ((1 << sensorGPIOs[i]) & report.level)) { // Work out how long it took, and indicate we've gotten // a result for this sensor sensorDiff[i] = report.tick - startTime; dataRead[i] = true; } // We may have gotten a spurious 'low', so if the GPIO goes high again then // reset the 'dataRead' for this sensor else if(dataRead[i] && ((1 << sensorGPIOs[i]) & report.level)) { dataRead[i] = false; } } } }
void i2cTick(void) { static int inited = 0; static int calibrated = 0; static int calibrations = 0; static int accCalibX = 0, accCalibY = 0, accCalibZ = 0; static int gyroCalibX = 0, gyroCalibY = 0, gyroCalibZ = 0; static int i2c; static float X=0.0, Y=0.0, Z=0.0; static uint32_t lastTick; uint32_t tick; int elapsed; int pulse; char str[256]; if (inited) { tick = gpioTick(); elapsed = tick - lastTick; lastTick = tick; readADXL345(i2c); readITG3200(i2c); if (calibrated) { X = estimateAngle( rawAcc[ROLL], rawGyr[ROLL] -gyroCalibX, X, elapsed); Y = estimateAngle( rawAcc[PITCH], rawGyr[PITCH] - gyroCalibY, Y, elapsed); Z = estimateAngle( rawAcc[YAW], rawGyr[YAW] - gyroCalibZ, Z, elapsed); pulse = 1500 + (Y * 1000 / 90); if (pulse < 500) pulse = 500; if (pulse > 2500) pulse = 2500; gpioServo(SERVO1, pulse); pulse = 1500 - (X * 500 / 90); if (pulse < 1000) pulse = 1000; if (pulse > 2000) pulse = 2000; gpioServo(SERVO2, pulse); /* prefer Z but that doesn't change much */ pulse = 1500 - (Y * 500 / 90); if (pulse < 800) pulse = 800; if (pulse > 2200) pulse = 2200; gpioServo(SERVO3, pulse); sprintf(str, "X=%4.0f Y=%4.0f Z=%4.0f ", X, Y, Z); putTTYstr(1, 1, str); } else { accCalibX+=rawAcc[ROLL]; accCalibY+=rawAcc[PITCH]; accCalibZ+=rawAcc[YAW]; gyroCalibX+=rawGyr[ROLL]; gyroCalibY+=rawGyr[PITCH]; gyroCalibZ+=rawGyr[YAW]; if (++calibrations >= CALIBRATIONS) { accCalibX /= CALIBRATIONS; accCalibY /= CALIBRATIONS; accCalibZ /= CALIBRATIONS; gyroCalibX /= CALIBRATIONS; gyroCalibY /= CALIBRATIONS; gyroCalibZ /= CALIBRATIONS; calibrated = 1; } } } else { i2c = initI2Cdevices(); gpioServo(SERVO1, 1500); gpioServo(SERVO2, 1500); gpioServo(SERVO3, 1500); inited = 1; } }