static uint16_t SPIReadADC(uint8_t channel)
{
uint8_t ii;
uint32_t result;
uint32_t setup;
//Setting up pins
// In case pin was already been low, we put it high
// so we can initiate communication after setting up pins
set_output(SPI_CS, HIGH);
set_output(SPI_DATA, LOW);
set_output(SPI_CLK, LOW);
low(SPI_CS); // Active chip select by setting pin low
// Sending configuration to device
setup = channel | 0b11000;
for(ii = 0; ii < 5; ++ii)
{
pulse_out(SPI_CLK, 1);
if ((setup & 0b10000) == 0b10000)
{
high(SPI_DATA);
}
else
{
low(SPI_DATA); // is MSB != 0
}
setup <<= 1; // shift left
}
pulse_out(SPI_CLK, HIGH); //Empty clock, for sampling
pulse_out(SPI_CLK, HIGH); //Device returns low, NULL bit, we ignore it...
input(SPI_DATA);
// read ADC result 12 bit
result = 0;
for(ii = 0; ii < 12; ++ii)
{
// We are sending pulse, clock signal, to ADC, because on falling edge it will return data...
pulse_out(SPI_CLK, HIGH);
// Shifting bit to left, to make room for current one...
result <<= 1;
result = result | (get_state(SPI_DATA) & 0x01);
}
high(SPI_CS);
}
int UltraSonicSensor::getDistance(){
pulse_out(pin,10);
int velocity = 331.5 + (0.6 * temperature);
int travelTime = pulse_in(pin,1);
int distance = (velocity * travelTime * 100)/(2 * 1000000);
if(2 < distance && distance < 330){
return distance;
}else{
return -1;
}
}
static float ReadUltrasonic(uint8_t addr)
{
// Set pins directions
set_direction(MUX_ADDR_START, OUTPUT);
set_direction(MUX_ADDR_START + 1, OUTPUT);
set_direction(MUX_ADDR_START+ 2, OUTPUT);
set_direction(MUX_ADDR_START + 3, OUTPUT);
set_direction(TRIG_PIN, OUTPUT);
set_direction(ECHO_PIN, INPUT);
// Set the mux based on sensor index
set_outputs(MUX_ADDR_END, MUX_ADDR_START, (int) addr);
// Pulse the trigger pin
low(TRIG_PIN);
pulse_out(TRIG_PIN, 10);
// Read in echo pulse
uint32_t pulse = pulse_in(ECHO_PIN, HIGH);
return pulse/58.0;
}
void servo(void *par) // Servo process in other cog
{
int s = sizeof(p)/sizeof(int); // Get size of servo array
int i; // Local index variable
mark(); // Mark the current time
while(1) // Servo control loop
{
while(lockset(lockID)); // Set the lock
for(i = 0; i < s; i++) // Go through all possible servos
{
if(t[i] == 0) // Detach servo?
{
input(p[i]); // Set I/O pin to input
p[i] = -1; // Remove from list
t[i] = -1;
}
if(p[i] != -1) // If servo entry in pin array
{
int tPulse = t[i]; // Copy requested position to var
int diff = tPulse - tp[i]; // Check size of change
int d = abs(diff); // Take absolute value
if(r[i] < d) // If change larger than ramp step
{
int step = r[i]; // Copy step entry to variable
if(diff < 0) // If negative
{
step = -step; // Make step negative
}
tPulse = tp[i] + step; // Increment pulse by step
}
pulse_out(p[i], tPulse); // Send pulse to servo
tp[i] = tPulse; // Remember pulse for next time
}
}
lockclr(lockID); // Clear the lock
wait(20000);
}
}
