void I2C_ByteRead(I2C_ADDR_t slaveAddress, uint8_t reg, uint8_t* data)
{
uint8_t num_bytes;
uint8_t size = 0;
if (reg > 0)
{
size = sizeof(reg);
}
lockset(lock);
while (i2c_busy(i2c_bus, slaveAddress))
{
;
}
num_bytes = i2c_in(i2c_bus, slaveAddress, reg, size, data, 1);
lockclr(lock);
print("\t\tRead addr %02x, reg %02x, data %02x, total %d\n", slaveAddress, reg, *data, num_bytes);
}
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);
}
}
/* Constructor which creates one lock on first construction.
* This is not completely thread safe, but these are created
* during program setup before backgroup processing starts.
*/
Servo::Servo()
{
// Now we're thread safe, so set the lock.
while(lockset(Servo::_lockID));
// if we still have free slots.
if( Servo::_servoCount < MAX_SERVOS)
{
// Grab a slot in the properties array.
_idx = Servo::_servoCount;
// Increment the servo count.
Servo::_servoCount++;
}
else
{
// too many servos
_idx = INVALID_SERVO;
}
// Clear lock
lockclr(Servo::_lockID);
}
/* Releases a pin from this servo object and resets the slot state.
*/
void Servo::detach()
{
// Set lock.
while(lockset(Servo::_lockID));
Servo::_slots[_idx]._pin = -1;
Servo::_slots[_idx]._currentPulse = DEFAULT_PULSE_WIDTH;
Servo::_slots[_idx]._previousPulse = DEFAULT_PULSE_WIDTH;
Servo::_slots[_idx]._stepSize = 2000;
// Decrement the attached servo count.
Servo::_attachedCount--;
// if no more attached servos and the cog is running,
// then stop the cog and record that it's stopped.
if (Servo::_attachedCount == 0 && Servo::_servoCog != 0)
{
cogstop(Servo::_servoCog-1);
Servo::_servoCog = 0;
}
// Clear lock
lockclr(Servo::_lockID);
}
/* 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);
}
void InitI2C()
{
lock = locknew();
lockclr(lock);
i2c_open(&i2c_bus, I2C_SCL, I2C_SDA, 0);
}
static void HandleSerial(void *par)
{
int count;
char next;
while (1)
{
// Check for incoming message, read and store it, signal the message is ready
if (!incoming_msg.is_full)
{
if (fdserial_rxReady(msg_serial) != 0)
{
count = 0;
while (count < sizeof(incoming_msg.data))
{
next = readChar(msg_serial);
lockset(lock);
incoming_msg.data[count] = next;
#ifdef ECHO_SERIAL_MSG // Use this only for debugging problems with message handling
echo_msg.data[count] = next;
#endif
lockclr(lock);
if (incoming_msg.data[count] == '\r' || incoming_msg.data[count] == '\n')
{
lockset(lock);
incoming_msg.count = count;
//incoming_msg[count + 1] = '\0';
incoming_msg.is_full = 1;
#ifdef ECHO_SERIAL_MSG // Use this only for debugging problems with message handling
echo_msg.count = count;
//echo_msg[count] = '\0';
echo_msg.is_full = 1;
#endif
lockclr(lock);
break;
}
count++;
}
}
}
else
{
// This means we aren't processing incomming messages fast enough
// Could implment an overflow counter that could be reported back as status
}
#ifdef ECHO_SERIAL_MSG // Use this only for debugging problems with message handling
lockset(lock);
if (echo_msg.is_full)
{
dprint(msg_serial, "E:%s", echo_msg.data);
echo_msg.is_full = 0;
}
lockclr(lock);
#endif
lockset(lock);
// Check for outgoing message, send, and clear flag
if (outgoing_msg.is_full)
{
dprint(msg_serial, "%s", outgoing_msg.data);
outgoing_msg.is_full = 0;
}
lockclr(lock);
}
}
/****************************** start main routine ******************************/
int main(void)
{
char input_buffer[INPUT_BUFFER]; //buffer for user input
waitcnt(500000 + _CNT); //wait until initialization is complete
printf("XMMC-cogc demo v%s",VERSION); //display startup message
/* start monitoring the real time clock DS3231 */
int cog;
rtc_cb.rtc.tdb_lock = locknew();
lockclr(rtc_cb.rtc.tdb_lock);
cog = start_rtc(&rtc_cb.rtc);
if(cog == -1)
{
printf("** error attempting to start rtc cog\n cognew returned %i\n\n",cog);
return 1;
}
printf(" DS3231 monitored by code running on cog %i\n",cog);
/* set all cogs to not running */
parA.A.cog = -1;
parB.B.cog = -1;
parC.C.cog = -1;
/* loop forever */
while(1)
{
printf("\nenter command > "); //prompt user
fgets(input_buffer,INPUT_BUFFER,stdin); //get a line of input
input_buffer[strlen(input_buffer)-1] = '\0'; //get rid of trailing new line character
switch(process(input_buffer)) //test input,take appropriate action
{
case 0: //startA
if(start_cogA(&parA.A)== -1)
printf(" problem starting cogA\n");
else
printf(" cogA started\n");
break;
case 1: //startB
if(start_cogB(&parB.B)== -1)
printf(" problem starting cogB\n");
else
printf(" cogB started\n");
break;
case 2: //startC
if(start_cogC(&parC.C)== -1)
printf(" problem starting cogC\n");
else
printf(" cogC started\n");
break;
case 3: //stopA
cogstop(parA.A.cog);
printf(" cog A stopped\n");
parA.A.cog = -1;
break;
case 4: //stopB
cogstop(parB.B.cog);
printf(" cog B stopped\n");
parB.B.cog = -1;
break;
case 5: //stopC
cogstop(parC.C.cog);
printf(" cog C stopped\n");
parC.C.cog = -1;
break;
case 6: //queryA
if(parA.A.cog == -1)
printf("cog A is not running\n");
else
parA.A.query_flag = 1;
break;
case 7: //queryB
if(parB.B.cog == -1)
printf("cog B is not running\n");
else
parB.B.query_flag = 1;
break;
case 8: //queryC
if(parC.C.cog == -1)
printf("cog C is not running\n");
else
parC.C.query_flag = 1;
break;
case 9: //status
status();
break;
case 10: //exit
printf("exiting program\n");
return 0;
case 11: //time
printf("%s, %i:%02i:%02i %i/%i/%i\n\n",
day_names_long[rtc_cb.rtc.dow-1],
rtc_cb.rtc.hour,
rtc_cb.rtc.min,
rtc_cb.rtc.sec,
rtc_cb.rtc.month,
rtc_cb.rtc.day,
rtc_cb.rtc.year+2000);
break;
default:
printf("<%s> is not a valid command\n",input_buffer);
}
}
return 0;
}
int main(void)
{
static int cog;
static char tbuf[_TOKEN_BUFFER];
char c;
/************************ initializations ****************************/
sleep(1); //wait for the serial terminal to start
/* display system info on serial terminal */
printf("\n*** Pcon %i.%i ***\n\n",_major_version,_minor_version);
#if _DRIVEN == _DIOB
printf("system is configured to drive a Parallax Digital IO Board\n");
#else
printf("system is configured to drive 5 IO pins\n");
#endif
/* build file set prefix */
strcat(file_set_prefix,_F_PREFIX);
strcat(file_set_prefix,_FILE_SET_ID);
printf("file set prefix <%s>\n",file_set_prefix);
/* check out the SD card */
if(sd_setup())
{
printf("**** sd_setup aborted application ****\n");
return 1;
}
/* start monitoring the real time clock DS3231 */
rtc_cb.rtc.tdb_lock = locknew();
lockclr(rtc_cb.rtc.tdb_lock);
cog = start_rtc(&rtc_cb.rtc);
if(cog == -1)
{
printf("** error attempting to start rtc cog\n cognew returned %i\n\n",cog);
return 1;
}
printf(" DS3231 monitored by code running on cog %i\n",cog);
/* setup the dio control block */
dio_cb.dio.tdb_lock = rtc_cb.rtc.tdb_lock;
dio_cb.dio.cca_lock = locknew();
lockclr(dio_cb.dio.cca_lock);
dio_cb.dio.sch_lock = locknew();
lockclr(dio_cb.dio.sch_lock);
dio_cb.dio.update_ptr = &(rtc_cb.rtc.update);
dio_cb.dio.td_ptr = &(rtc_cb.rtc.td_buffer);
*dio_cb.dio.update_ptr = 0;
dio_cb.dio.sch_ptr = bbb;
/* start the dio cog */
cog = start_dio(&dio_cb.dio);
if(cog == -1)
{
printf("** error attempting to start dio cog\n cognew returned %i\n\n",cog);
return 1;
}
#if _DRIVEN == _DIOB
printf(" DIO Board controlled by code running on cog %i\n",cog);
#else
printf(" relays controlled by code running on cog %i\n",cog);
#endif
/* set up unbuffered nonblocking io */
setvbuf(stdin, NULL, _IONBF, 0);
setvbuf(stdout, NULL, _IONBF, 0);
stdin->_flag &= ~_IOCOOKED;
stdin->_flag |= _IONONBLOCK;
/* initialize fsm(s) parameters */
input_buffer_ptr = input_buffer;//setup so actions routines can mess the buffer
cmd_state = 0; //set initial command parser state
char_state = 0; //set initial caracter parser state
// hold_day = -1; //force schedule load first time through the main loop
*input_buffer = ' '; //load a a blank into the buffer to force first prompt
process_buffer();
printf("initialization complete\n");
/************************************************************/
/********************** main processing loop ****************/
/************************************************************/
while(1)
{
/* check for problems */
if(ckeck_abort())
return 1;
/* check the token stack */
while(pop_cmd_q(tbuf))
{
cmd_fsm(tbuf,&cmd_state); //cycle cmd fsm until queue is empty
}
/* grab a character from the keyboard if one is present */
c = fgetc(stdin); //nonblocking read
/* process input char */
if(c!=_NO_CHAR)
{
fputc(c, stdout); // echo char
if(c==_CR) fputc(_CR, stdout); //second CR after uer input
char_fsm(char_type(c),&char_state,&c); //cycle fsm
}
};
printf("\nnormal termination\n\n");
return 0;
}