void ee_writeStr(char *s, int n, int addr)
{
while(lockset(eei2cLock));
ee_putStr(s, n, addr);
lockclr(eei2cLock);
return;
}
void init_MMA7660FC(void)
{
int x, y, z;
unsigned char val = 0;
bt_accelInitFlag = 1;
if(!eei2cLockFlag)
{
leds(0b111111);
eei2cLock = locknew();
lockclr(eei2cLock);
eei2cLockFlag = 1;
}
if(!st_eeInitFlag) ee_init();
while(lockset(eei2cLock));
i2c_out(st_eeprom, MMA7660_I2C,
MODE, 1, &val, 1);
i2c_out(st_eeprom, MMA7660_I2C,
INTSU, 1, &val, 1);
i2c_out(st_eeprom, MMA7660_I2C,
SR, 1, &val, 1);
val = 0xC1;
i2c_out(st_eeprom, MMA7660_I2C,
MODE, 1, &val, 1);
i2c_stop(st_eeprom);
lockclr(eei2cLock);
accels(&x, &y, &z);
}
short ee_readShort(int addr)
{
while(lockset(eei2cLock));
int value;
i2c_in(st_eeprom, 0x50, addr, 2, (unsigned char*) &value, 2);
lockclr(eei2cLock);
return value;
}
uint8_t I2CBusy(uint8_t address)
{
uint8_t result;
lockset(lock);
result = i2c_busy(i2c_bus, address);
lockclr(lock);
return result;
}
void SendMessage(char* buffer)
{
PEND_ON_BUFFER(outgoing_msg);
lockset(lock);
memcpy((char *) outgoing_msg.data, buffer, strlen(buffer));
outgoing_msg.data[strlen(buffer)] = 0; // Null terminate the data
outgoing_msg.is_full = 1;
lockclr(lock);
}
/* Sets the ramp rate.
*/
void Servo::setRamp(int16_t stepSize)
{
// Set the lock
while(lockset(Servo::_lockID));
Servo::_slots[_idx]._stepSize = stepSize;
// Clear the lock before return.
lockclr(Servo::_lockID);
}
void Servo::writeMicroseconds(int16_t value)
{
// Set the lock
while(lockset(Servo::_lockID));
Servo::_slots[_idx]._currentPulse = value;
// Clear the lock before return.
lockclr(Servo::_lockID);
}
static void servo(void *par) // servo process in other cog
{
int dtpw = (CLKFREQ/1000000)*2500;
int pw = CNT;
int dtTw = (CLKFREQ/1000000)*20000;
int Tw = pw;
Tw += dtTw;
int s = sizeof(p)/sizeof(int); // Get size of servo array
int i; // Local index variable
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;
tp[i] = -1;
r[i] = 2000;
}
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_outCtr(p[i], tPulse); // Send pulse to servo
tp[i] = tPulse; // Remember pulse for next time
}
if(i%2)
{
while((CNT - pw) <= dtpw); // Wait for servo pulse window to close
pw += dtpw;
}
}
lockclr(lockID); // Clear the lock
while((CNT - pw) <= dtpw); // Wait for 20 ms since first pulse
pw += dtpw;
}
}
int accel(int axis)
{
if(!st_eeInitFlag) ee_init();
unsigned char val = 0;
while(lockset(eei2cLock));
i2c_in (st_eeprom, MMA7660_I2C,
axis, 1, &val, 1);
i2c_stop(st_eeprom);
lockclr(eei2cLock);
int g100 = raw2g100(val);
if(axis == AY) g100 = -g100;
return g100;
}
/* Returns true if the pin has a value.
*/
bool Servo::attached()
{
bool retVal;
// Set the lock
while(lockset(Servo::_lockID));
// capture the current pulse value.
retVal = Servo::_slots[_idx]._pin != -1 ? true : false;
lockclr(Servo::_lockID);
return retVal;
}
/* Returns the current pulse value in microseconds, note that if ramping
* is in use this value might not be the value being sent to the servo.
*/
int16_t Servo::readMicroseconds()
{
int retVal = -1;
// Set the lock
while(lockset(Servo::_lockID));
// capture the current pulse value.
retVal = Servo::_slots[_idx]._currentPulse;
lockclr(Servo::_lockID);
return retVal;
}
uint8_t AcquireMessage(char *buffer)
{
if (incoming_msg.is_full)
{
lockset(lock);
memcpy(buffer, (char *) incoming_msg.data, incoming_msg.count);
//incoming_msg.data[incoming_msg.count + 1] = 0;
incoming_msg.is_full = 0;
lockclr(lock);
return 1;
}
return 0;
}
/* servo tending cog method.
*/
void Servo::servoLoop(void *par)
{
// Servo control loop runs until stopped.
while(true)
{
// For each servo.
for(uint8_t i = 0; i < MAX_SERVOS; i++)
{
// Set the lock
while(lockset(Servo::_lockID));
// If this servo slot is bound to a pin.
if (Servo::_slots[i]._pin != -1)
{
// Copy requested position to var
int16_t tPulse = Servo::_slots[i]._currentPulse;
// Compute the required size of change.
int16_t diff = tPulse - Servo::_slots[i]._previousPulse;
// If change larger than ramp step
if (Servo::_slots[i]._stepSize < abs(diff))
{
int16_t step = Servo::_slots[i]._stepSize;
// If the difference is negative, then make the step negative.
if (diff < 0)
{
step = -step;
}
// Increment pulse by step to get closer to target.
tPulse = Servo::_slots[i]._previousPulse + step;
}
// Send pulse to servo and remember pulse for next time
pinMode(Servo::_slots[i]._pin, OUTPUT);
pulseOut(Servo::_slots[i]._pin, tPulse);
Servo::_slots[i]._previousPulse = tPulse;
}
// Clear the lock.
lockclr(Servo::_lockID);
}
// Sleep until next pulse required.
delay(20);
}
}
int32_t test29::Str(int32_t Stringptr)
{
while (lockset(Strlock)) {
Yield__();
}
{
int32_t _idx__0000;
int32_t _limit__0001 = strlen((char *) Stringptr);
for(_idx__0000 = 1; _idx__0000 <= _limit__0001; (_idx__0000 = (_idx__0000 + 1))) {
Tx(((uint8_t *)(Stringptr++))[0]);
}
}
lockclr(Strlock);
return 0;
}
int servo_setramp(int pin, int stepSize) // Set ramp step for a servo
{
int s = sizeof(p)/sizeof(int); // Get array size
int i; // Local index variable
while(lockset(lockID)); // Set lock
for(i = 0; i < s; i++) // Find index for servo pin
{
if(p[i] == pin) // Found pin in array?
{
r[i] = stepSize; // Set ramp step
lockclr(lockID); // Clear lock
return pin; // Return success
}
}
lockclr(lockID); // Clear lock
return -4; // Return -1, pin not found
}
int32_t screen_update(void)
{
while(lockset(screenLock));
int32_t _local__0014[2] = {0, 0};
// Writes the screen buffer to the memory of the display
// low col = 0
screen_ssd1306_Command(SSD1306_SETLOWCOLUMN);
// hi col = 0
screen_ssd1306_Command(SSD1306_SETHIGHCOLUMN);
// line #0
screen_ssd1306_Command(SSD1306_SETSTARTLINE);
screen_HIGH(self->DC);
screen_WRITEBUFF(self->DATA, self->CLK, self->CS, (int32_t)(&_local__0014[1]), (int32_t)(&self->buffer[0]));
//screen_WRITEBUFF(self->DATA, self->CLK, self->CS, 8192, (int32_t)(&self->buffer[0]));
screen_LOW(self->DC);
lockclr(screenLock);
return 0;
}
uint16_t I2C_ReadADC(uint8_t write_addr, uint8_t read_addr, uint8_t channel)
{
uint8_t a2d_val_high;
uint8_t a2d_val_low;
int ack;
lockset(lock);
ack = i2c_startpoll(&i2c_bus, write_addr);
ack = i2c_writeByte(&i2c_bus, ADC_CHANNEL[channel]);
ack = i2c_startpoll(&i2c_bus, read_addr);
a2d_val_high = i2c_readByte(&i2c_bus, 0);
a2d_val_low = i2c_readByte(&i2c_bus, 1);
i2c_stop(&i2c_bus);
lockclr(lock);
return (((uint16_t) a2d_val_high) << 8) | (uint16_t) a2d_val_low;
}
void I2C_ByteWrite(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);
if (I2C_Ready(slaveAddress))
{
num_bytes = i2c_out(i2c_bus, slaveAddress, reg, size, &data, sizeof(data));
}
lockclr(lock);
print("\t\tWrote - addr %02x, reg %02x, data %02x, total %d\n", slaveAddress, reg, data, num_bytes);
}
int servo_set(int pin, int time) // Set pulse width to servo on pin
{
if(servoCog == 0) // If cog not started
{
int result = servo_start(); // Start the cog
if(result == 0) return 0; // No cogs open
if(result == -1) return -1; // No locks open
}
int s = sizeof(p)/sizeof(int); // Array size to s
int i; // Index variable
while(lockset(lockID)); // Set the lock
for(i = 0; i < s; i++) // Check if existing servo
{
if(p[i] == pin) // Yes?
{
t[i] = time; // Set pulse duration
lockclr(lockID); // Clear lock
return 1; // Return success
}
}
if(i == s) // Not existing servo?
{
for(i= 0; i < s; i++) // Look for empty slot
{
if(p[i]==-1) // Found one?
{
break; // Exit for loop, keep index
}
}
if(i <= s) // Found empty slot?
{
p[i] = pin; // Set up pin and pulse durations
t[i] = time;
tp[i] = time;
lockclr(lockID); // Clear the lock
return 1; // Return success
}
else // Servo not found, no empty slots?
{
lockclr(lockID); // Clear lock
return 0; // Return, set not completed
}
}
}
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);
}
/* 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);
}
/* 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);
}
void AsiMS2000::selectCommand(int commandNum)
{
switch(commandNum)
{
case 0:
accel();
break;
case 1:
aalign();
break;
case 2:
afcont();
break;
case 3:
aflim();
break;
case 4:
afocus();
break;
case 5:
afset();
break;
case 6:
afmove();
break;
case 7:
ahome();
break;
case 8:
aij();
break;
case 9:
array();
break;
case 10:
azero();
break;
case 11:
backlash();
break;
case 12:
bcustom();
break;
case 13:
benable();
break;
case 14:
build();
break;
case 15:
cdate();
break;
case 16:
cnts();
break;
case 17:
customa();
break;
case 18:
customb();
break;
case 19:
dack();
break;
case 20:
dump();
break;
case 21:
ensync();
break;
case 22:
epolarity();
break;
case 23:
error();
break;
case 24:
halt();
break;
case 25:
here();
break;
case 26:
home();
break;
case 27:
info();
break;
case 28:
joystick();
break;
case 29:
jsspd();
break;
case 30:
kadc();
break;
case 31:
kd();
break;
case 32:
ki();
break;
case 33:
kp();
break;
case 34:
lcd();
break;
case 35:
led();
break;
case 36:
lladdr();
break;
case 37:
load();
break;
case 38:
lock();
break;
case 39:
lockrg();
break;
case 40:
lockset();
break;
case 41:
maintain();
break;
case 42:
motctrl();
break;
case 43:
move();
break;
case 44:
movrel();
break;
case 45:
pcros();
break;
case 46:
pedal();
break;
case 47:
rbmode();
break;
case 48:
rdadc();
break;
case 49:
rdsbyte();
break;
case 50:
rdstat();
break;
case 51:
relock();
break;
case 52:
reset();
break;
case 53:
rt();
break;
case 54:
runaway();
break;
case 55:
saveset();
break;
case 56:
savepos();
break;
case 57:
scan();
break;
case 58:
scanr();
break;
case 59:
scanv();
break;
case 60:
secure();
break;
case 61:
sethome();
break;
case 62:
setlow();
break;
case 63:
setup();
break;
case 64:
si();
break;
case 65:
speed();
break;
case 66:
spin();
break;
case 67:
status();
break;
case 68:
stopbits();
break;
case 69:
ttl();
break;
case 70:
um();
break;
case 71:
units();
break;
case 72:
unlock();
break;
case 73:
vb();
break;
case 74:
vector();
break;
case 75:
version();
break;
case 76:
wait();
break;
case 77:
where();
break;
case 78:
who();
break;
case 79:
wrdac();
break;
case 80:
zero();
break;
case 81:
z2b();
break;
case 82:
zs();
break;
case 83:
overshoot();
break;
}
}
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);
}
}
