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