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;
}
}
