int main() // Main - execution begins!
{
xbee = fdserial_open(11, 10, 0, 9600);
//dprint(xbee, "hello");
//dprint(xbee, "hello");
drive_speed(0,0); // Start servos/encoders cog
drive_setRampStep(10); // Set ramping at 10 ticks/sec per 20 ms
sirc_setTimeout(50); // Remote timeout = 50 ms
//drive_feedback(0);
dt = CLKFREQ/10;
t = CNT;
while(1) // Outer loop
{
int button = sirc_button(4); // check for remote key press
// Motion responses - if key pressed, set wheel speeds
if(button == 2)
{
if(!running)
cogstart(&gofwd, NULL, fstack, sizeof fstack);
}
if(button == 8)
{
if(!running)
cogstart(&gobkwd, NULL, bstack, sizeof bstack);
}
if(button == CH_UP)drive_rampStep(100, 100); // Left turn
if(button == CH_DN)drive_rampStep(-100, -100); // Right turn
if(button == VOL_DN)drive_rampStep(-100, 100); // Left turn
if(button == VOL_UP)drive_rampStep(100, -100); // Right turn
if(button == MUTE)drive_rampStep(0, 0); // Stop
if(button == ENTER)
{
getTicks();
displayTicks();
}
if(CNT - t > dt)
{
t+=dt;
i++;
getTicks();
if
(
ticksLeftCalc != ticksLeftCalcOld
|| ticksRightCalc != ticksRightCalcOld
|| ticksLeft != ticksLeftOld
|| ticksRight != ticksRightOld
)
{
displayTicks();
}
}
}
}
int main()
{
// Close the simpleterm half duplex connection
simpleterm_close();
// Start the sensor cog(s)
cogstart(&pollPingSensors, NULL, pstack, sizeof pstack);
// Blinken Lights!
const int pauseTime = 50;
const int startLED = 17; // 16 is used for the pollPingSensors cog to indicate activity.
const int endLED = 23;
high(startLED);
pause(pauseTime);
low(startLED);
while(1) {
pause(5);
if(isActive == 1) {
for (int led = startLED + 1; led <= endLED; led++) {
high(led);
pause(pauseTime);
low(led);
}
for (int led = endLED - 1; led >= startLED; led--) {
high(led);
pause(pauseTime);
low(led);
}
isActive = 0;
}
}
}
void square_wave(int pin, int channel, int freq)
{
if(!cog) cog = cogstart(&square_wave_cog, NULL,
stack, sizeof(stack)) + 1;
int ctr, frq;
square_wave_setup(pin, freq, &ctr, &frq);
if(!channel)
{
ctra = ctr;
frqa = frq;
if(pin < 0)
{
pin = -pin;
ctra = 0;
frqb = 0;
}
}
else
{
ctrb = ctr;
frqb = frq;
if(pin < 0)
{
pin = -pin;
ctrb = 0;
frqb = 0;
}
//printf("ctrb = %s\n", itoa(ctrb, s, 2));
//printf("frqb = %s\n", itoa(frqb, s, 2));
}
}
int pwm_start(unsigned int cycleMicroseconds)
{
//us = CLKFREQ/1000000;
tCycle = cycleMicroseconds * st_usTicks;
pwcog = cogstart(pw, NULL, pwstack, sizeof(pwstack)) + 1;
return pwcog;
}
void MessageStart()
{
lock = locknew();
lockclr(lock);
simpleterm_close();
msg_serial = fdserial_open(31, 30, 0, 115200);
cogstart(&HandleSerial, NULL, msg_stack, sizeof(msg_stack));
}
void SensorsStart()
{
uint8_t ii;
memset(&SensorState, 0, sizeof(SensorState));
cogstart(&PollSensors, NULL, sensor_stack, sizeof(sensor_stack));
}
int servo_start(void) // Take cog & start servo process
{
lockID = locknew(); // Check out a lock
if (lockID == -1) return -1; // Return -1 if no locks
servo_stop(); // Stop in case cog is running
servoCog = cogstart(&servo, NULL, stack, // Launch servo into new cog
sizeof(stack)) + 1;
return servoCog; // Return cog that was taken
}
void dac_ctr(int pin, int channel, int dacVal)
{
if(dacCtrBits == 0) dacCtrBits = 8;
int dacBitX = 32 - dacCtrBits;
if(!cog) cog = cogstart(&dac_ctr_cog, NULL,
stack, sizeof(stack)) + 1;
if(!channel)
{
ctra = (DUTY_SE + pin);
frqa = (dacVal << dacBitX);
}
else
{
ctrb = (DUTY_SE + pin);
frqb = (dacVal << dacBitX);
}
}
int *cog_runStackTest(void (*function)(void *par), int stacksize)
{
srand(stacksize);
//srand(0);
int stackSizeBytes = stacksize * (sizeof(int));
//print("stackSizeBytes = %d bytes, %d ints\n", stackSizeBytes, stackSizeBytes/sizeof(int));
int stackOverhead = sizeof(_thread_state_t) + (3 * sizeof(unsigned int));
//print("stackOverhead = %d bytes, %d ints\n", stackOverhead, stackOverhead/sizeof(int));
int cogRunTestOverhead = 2 * sizeof(int);
//print("cogRunTestOverhead = %d bytes, %d ints\n", cogRunTestOverhead, cogRunTestOverhead/sizeof(int));
int stackBytes = stackSizeBytes + stackOverhead + cogRunTestOverhead;
//print("stackBytes = %d bytes, %d ints\n", stackBytes, stackBytes/sizeof(int));
int stackInts = stackBytes / sizeof(int);
//print("stackInts = %d ints\n", stackInts);
int *addr;
addr = malloc(stackBytes);
int n = 0;
for(int *i = addr+2; i < (addr + (stackInts)); i++)
{
// *i = n;
*i = rand();
n++;
}
*addr = 1 + cogstart(function, NULL, addr + cogRunTestOverhead/sizeof(int), stackBytes - cogRunTestOverhead);
if(*addr == 0)
{
free(addr);
return (int*) 0;
}
//print("\n\n---[ cog_run ]--- \n\n");
//print("Cog Address = %d, Cog Value = %d\n", (int) addr, *addr);
*(addr+1) = stackInts;
//print("Stack Count Address = %d, Stack Count = %d\n\n", (int)(addr+1), *(addr+1));
// // print("(int)(addr+2) = %d\n", (int)(addr+2));
return addr;
}
void da_out(int channel, int daVal)
{
if(abvolts_daCtrBits == 0) abvolts_daCtrBits = 8;
int daBitX = 32 - abvolts_daCtrBits;
if(!cog)
{
cog = cogstart(&da_ctr_cog, NULL,
stack, sizeof(stack)) + 1;
}
if(!channel)
{
ctra = (DUTY_SE + pinCh0);
frqa = (daVal << daBitX);
}
else
{
ctrb = (DUTY_SE + pinCh1);
frqb = (daVal << daBitX);
}
}
int gps_open(int gpsSin, int gpsSout, int gps_baud) // Open reader, start reading
{
stopping = 0;
cog = cogstart(&gps_run, NULL, stack, sizeof(stack));
if(cog < 0)
{
//a valid cog was NOT grabbed, clear the GPS data structure and pin info
memset(&gps_data, 0, sizeof(nmea_data));
memset(&_gps_rx_pin, 0, (sizeof(int)*3));
}
else
{
//the GPS parser cog was started
_gps_rx_pin = gpsSin;
_gps_tx_pin = gpsSout;
_gps_baud = gps_baud;
}
return(cog < 0 ? FALSE:TRUE);
}
/* Binds a pin to this servo object and sets the limits.
*/
uint8_t Servo::attach(int16_t pin, int16_t min, int16_t max)
{
// capture the config for future write methods.
_min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
_max = (MAX_PULSE_WIDTH - max)/4;
// Set lock before touching the shared properties array
while(lockset(Servo::_lockID));
// If cog not started, start it.
if (Servo::_servoCog == 0)
{
Servo::_servoCog = cogstart(&servoLoop, NULL, stack, sizeof(stack)) + 1;
}
Servo::_slots[_idx]._pin = pin;
pinMode(Servo::_slots[_idx]._pin, OUTPUT);
// Increment when a servo is attached.
Servo::_attachedCount++;
// Clear lock
lockclr(Servo::_lockID);
}
int mstime_start()
{
mstime_stop();
cog = 1 + cogstart(&ms_timer, NULL, stack, sizeof(stack));
}
/* Start SpeedControl function in separate cog*/
void initMotorControl(void){
int cog = cogstart(&motorControl, NULL, stack, sizeof(stack));
}
int pwm_start(unsigned int cycleMicroseconds)
{
us = CLKFREQ/1000000;
tCycle = cycleMicroseconds * us;
pwcog = cogstart(&pw, NULL, pwstack, sizeof(pwstack)) + 1;
}
