void MAX7219::writeRegisters(const word *registers, byte size, byte chip) {
digitalWrite(_pinLOAD, LOW);
//Datasheet calls for 25ns between LOAD/#CS going low and the start of the
//transfer, Arduino can only go as low as 3us
delayMicroseconds(5);
for(byte i = 0; i < _chips - (chip + size); i++) injectNoop();
for(byte i = size; i > 0; i--) {
SPI.transfer(highByte(registers[i - 1]));
SPI.transfer(lowByte(registers[i - 1]));
}
for(byte i = 0; i < chip; i++) injectNoop();
digitalWrite(_pinLOAD, HIGH);
#if defined(MAX7219_DEBUG)
Serial.print("Wrote (register, value) pairs {");
for(word i = 0; i < size; i++) {
Serial.print("(0x");
Serial.print(highByte(registers[i]), HEX);
Serial.print(", 0x");
Serial.print(lowByte(registers[i]), HEX);
Serial.print(" [");
Serial.print(lowByte(registers[i]), BIN);
Serial.print("])");
if(i < size - 1) Serial.print(", ");
}
Serial.print("} starting at chip ");
Serial.println(chip);
#endif
}
//sends sensor data to mobile device
void DashBot::sendInfoPacket(void){
infoPacketTime = millis();
Serial1.write(INFO_PACKET); //2 means this is an info packet
Serial1.write(mode);
// send
int currentYaw = readGyroDeg();
// int sign = 1;
if (currentYaw < 0){
currentYaw = -currentYaw + 2000;
}
Serial1.write(highByte(currentYaw));
Serial1.write(lowByte(currentYaw));
int currentAmbientLight = readAmbientLight();
Serial1.write(highByte(currentAmbientLight));
Serial1.write(lowByte(currentAmbientLight));
int currentProxLeft = readLeftIRsensor();
Serial1.write(highByte(currentProxLeft));
Serial1.write(lowByte(currentProxLeft));
int currentProxRight = readRightIRsensor();
Serial1.write(highByte(currentProxRight));
Serial1.write(lowByte(currentProxRight));
Serial1.write(motor_right_backward_value);
Serial1.write(motor_right_forward_value);
Serial1.write(motor_left_backward_value);
Serial1.write(motor_left_forward_value);
}
// Oblify Packet Format...
// ---------------------------------------------------
// | HEADER | TYPE | FLAG | DevID | Dest | PAYLOAD |
// ---------------------------------------------------
// | 0|1|2|3 | 4|5 | 6 | 7|8 | 9|10 | 11...504 |
void sendPacketHeader(uint16_t type, uint8_t flag)
{
//currentMillis = millis();
//BLOCK UNTIL FLUSH TIME IS OVER
//Flush timer is 5ms by default
//while ((millis() - lastSentMillis) < 10) { }
//lastSentMillis = millis();
sentBytes += 11;
#ifdef OBLIFY
Serial.write(Header[0]);
Serial.write(Header[1]);
Serial.write(Header[2]);
Serial.write(Header[3]);
Serial.write(lowByte(type));
Serial.write(highByte(type));
Serial.write(flag);
Serial.write(lowByte(deviceID));
Serial.write(highByte(deviceID));
Serial.write(255);
Serial.write(255);
#else
Serial.print("Send: {oblify}");
Serial.print("{");
Serial.print(type);
Serial.print("}");
Serial.print("{");
Serial.print(flag);
Serial.print("}");
Serial.print("{");
Serial.print(deviceID);
Serial.print("}");
Serial.print("{65535}");
#endif
}
/**
* Sends the byte buffer. (Overloading from superclass)
*/
void ClientDbxZonePro::_sendBuffer() {
if (_bufferWriteIndex) {
_binarybuffer[0] = 0x01;
_binarybuffer[4] = _bufferWriteIndex;
_binarybuffer[5] = highByte(_localNodeId);
_binarybuffer[6] = lowByte(_localNodeId);
_binarybuffer[11] = highByte(_zoneProNodeId);
_binarybuffer[12] = lowByte(_zoneProNodeId);
_client.write(_binarybuffer, _bufferWriteIndex);
if (_serialOutput>2) {
Serial.print(F("_sendBuffer: \n"));
for(uint8_t i=0; i<_bufferWriteIndex; i++) {
if (_binarybuffer[i]<16) {
Serial.print(0);
}
Serial.print(_binarybuffer[i],HEX);
Serial.print(':');
if (i==20) {
Serial.print(' ');
}
}
Serial.print(F(" Length="));
Serial.println(_bufferWriteIndex);
}
_resetBuffer();
_pendingAnswer = true;
}
}
/**
* Internal function used to set the value of a registered state variable (both used for incoming data as well as outgoing data.)
*/
void ClientDbxZonePro::_setSVValue(uint8_t b0, uint8_t b1, uint8_t b2, uint8_t b3, uint16_t SV_ID, uint16_t value) {
for(uint8_t a=0; a<_SVnumStored; a++) {
if ( _SVstore[a*8+0]==b0 &&
_SVstore[a*8+1]==b1 &&
_SVstore[a*8+2]==b2 &&
_SVstore[a*8+3]==b3 &&
_SVstore[a*8+4]==highByte(SV_ID) &&
_SVstore[a*8+5]==lowByte(SV_ID)
) {
_SVstore[a*8+6] = highByte(value);
_SVstore[a*8+7] = lowByte(value);
if (_serialOutput>1) {
Serial.print(F("_setSVValue("));
Serial.print(b0);
Serial.print(F(","));
Serial.print(b1);
Serial.print(F(","));
Serial.print(b2);
Serial.print(F(","));
Serial.print(b3);
Serial.print(F(","));
Serial.print(SV_ID);
Serial.print(F("="));
Serial.print(value);
Serial.print(F(")\n"));
}
return;
}
}
}
void RegisterList::writeCVBitMain(char *s) volatile{
byte b[6]; // save space for checksum byte
int cab;
int cv;
int bNum;
int bValue;
byte nB=0;
if(sscanf(s,"%d %d %d %d",&cab,&cv,&bNum,&bValue)!=4)
return;
cv--;
bValue=bValue%2;
bNum=bNum%8;
if(cab>127)
b[nB++]=highByte(cab) | 0xC0; // convert train number into a two-byte address
b[nB++]=lowByte(cab);
b[nB++]=0xE8+(highByte(cv)&0x03); // any CV>1023 will become modulus(1024) due to bit-mask of 0x03
b[nB++]=lowByte(cv);
b[nB++]=0xF0+bValue*8+bNum;
loadPacket(0,b,nB,4);
} // RegisterList::writeCVBitMain()
// write 4 bytes to Sprite Control dwords to in RAM_SPR to set x/y pos to 400: offscreen
void vis_gd_xhide()
{
vis_spi_transfer(lowByte(400));
vis_spi_transfer(highByte(400));
vis_spi_transfer(lowByte(400));
vis_spi_transfer(highByte(400));
current_spr++;
}
void GDClass::xhide()
{
SPI.transfer(lowByte(400));
SPI.transfer(highByte(400));
SPI.transfer(lowByte(400));
SPI.transfer(highByte(400));
spr++;
}
void masterToSlave::sendXY(int posX, int posY){
Wire.beginTransmission(8); // transmit to device #8
Wire.write("@"); // sends bytes
Wire.write(highByte(posX)); // sends one byte from Xposition integer
Wire.write(lowByte(posX)); // sends one byte from Xposition integer
Wire.write(highByte(posY)); // sends one byte from Yposition integer
Wire.write(lowByte(posY)); // sends one byte from Yposition integer
Wire.endTransmission(); // stop transmitting
}
int AX12::setPosVel (int pos, int vel) {
processValue (GOAL_POSITION, &pos);
byte values [4];
values [0] = lowByte(pos);
values[1] = highByte(pos);
values [2] = lowByte(vel);
values[3] = highByte(vel);
return writeData (GOAL_POSITION, 4, values);
}
void sendEncoderData(uint8 msgid){
// addEncoderData(0x01,0x2,0x03,0x04);
// addEncoderData(0x01,0x2,0x03,0x04);//make sure it adds properly
int motorTicks1 = getMotor1Ticks(); //left
int motorTicks2 = getMotor2Ticks(); //right
addEncoderData(highByte(motorTicks1), lowByte(motorTicks1), highByte(motorTicks2), lowByte(motorTicks2));
sendFrameData(msgid);
setResetEncoderData(); //we want to reset the encoder data in main now that
}
// write sprite control lines, 32 bits, mostly used for single line, fixed address writing
void vis_gd_sprite(int spr, int x, int y, byte image, byte palette, byte rot, byte jk)
{
vis_gd_wstart(RAM_SPR + (spr << 2));
vis_spi_transfer(lowByte(x));
vis_spi_transfer((palette << 4) | (rot << 1) | (highByte(x) & 1));
vis_spi_transfer(lowByte(y));
vis_spi_transfer((jk << 7) | (image << 1) | (highByte(y) & 1));
vis_gd_end();
}
void GDClass::sprite(int spr, int x, int y, byte image, byte palette, byte rot, byte jk)
{
__wstart(RAM_SPR + (spr << 2));
SPI.transfer(lowByte(x));
SPI.transfer((palette << 4) | (rot << 1) | (highByte(x) & 1));
SPI.transfer(lowByte(y));
SPI.transfer((jk << 7) | (image << 1) | (highByte(y) & 1));
__end();
}
void BnrOneA::movePID(int speedL,int speedR)
{
byte speedL_H=highByte(speedL);
byte speedL_L=lowByte(speedL);
byte speedR_H=highByte(speedR);
byte speedR_L=lowByte(speedR);
byte buffer[]={KEY1,KEY2,speedL_H,speedL_L,speedR_H,speedR_L};
spiSendData(COMMAND_MOVE_PID,buffer,sizeof(buffer));
delay(5);//Wait while command is processed
}
void BnrOneA::minBat(float batmin)
{
int intg=(int)batmin;
int dec=(int)((batmin-intg)*1000);
byte intg_H=highByte(intg);
byte intg_L=lowByte(intg);
byte dec_H=highByte(dec);
byte dec_L=lowByte(dec);
byte buffer[]={KEY1,KEY2,intg_H,intg_L,dec_H,dec_L};
spiSendData(COMMAND_BAT_MIN,buffer,sizeof(buffer));
delay(25);//Wait while command is processed
}
void RegisterList::writeCVByte(char *s) volatile{
byte bWrite[4];
int bValue;
int c,d,base;
int cv, callBack, callBackSub;
if(sscanf(s,"%d %d %d %d",&cv,&bValue,&callBack,&callBackSub)!=4) // cv = 1-1024
return;
cv--; // actual CV addresses are cv-1 (0-1023)
bWrite[0]=0x7C+(highByte(cv)&0x03); // any CV>1023 will become modulus(1024) due to bit-mask of 0x03
bWrite[1]=lowByte(cv);
bWrite[2]=bValue;
loadPacket(0,resetPacket,2,1);
loadPacket(0,bWrite,3,4);
loadPacket(0,resetPacket,2,1);
loadPacket(0,idlePacket,2,10);
c=0;
d=0;
base=0;
for(int j=0;j<ACK_BASE_COUNT;j++)
base+=analogRead(CURRENT_MONITOR_PIN_PROG);
base/=ACK_BASE_COUNT;
bWrite[0]=0x74+(highByte(cv)&0x03); // set-up to re-verify entire byte
loadPacket(0,resetPacket,2,3); // NMRA recommends starting with 3 reset packets
loadPacket(0,bWrite,3,5); // NMRA recommends 5 verfy packets
loadPacket(0,resetPacket,2,1); // forces code to wait until all repeats of bRead are completed (and decoder begins to respond)
for(int j=0;j<ACK_SAMPLE_COUNT;j++){
c=(analogRead(CURRENT_MONITOR_PIN_PROG)-base)*ACK_SAMPLE_SMOOTHING+c*(1.0-ACK_SAMPLE_SMOOTHING);
if(c>ACK_SAMPLE_THRESHOLD)
d=1;
}
if(d==0) // verify unsuccessful
bValue=-1;
INTERFACE.print("<r");
INTERFACE.print(callBack);
INTERFACE.print("|");
INTERFACE.print(callBackSub);
INTERFACE.print("|");
INTERFACE.print(cv+1);
INTERFACE.print(" ");
INTERFACE.print(bValue);
INTERFACE.print(">");
} // RegisterList::writeCVByte()
void GDClass::screenshot(unsigned int frame)
{
int yy, xx;
byte undone[38]; // 300-long bitmap of lines pending
// initialize to 300 ones
memset(undone, 0xff, 37);
undone[37] = 0xf;
int nundone = 300;
Serial.write(0xa5); // sync byte
Serial.write(lowByte(frame));
Serial.write(highByte(frame));
while (nundone) {
// find a pending line a short distance ahead of the raster
int hwline = GD.rd16(SCREENSHOT_Y) & 0x1ff;
for (yy = (hwline + 7) % 300; ((undone[yy>>3] >> (yy&7)) & 1) == 0; yy = (yy + 1) % 300)
;
GD.wr16(SCREENSHOT_Y, 0x8000 | yy); // ask for it
// housekeeping while waiting: mark line done and send yy
undone[yy>>3] ^= (1 << (yy&7));
nundone--;
Serial.write(lowByte(yy));
Serial.write(highByte(yy));
while ((GD.rd(SCREENSHOT_Y + 1) & 0x80) == 0)
;
// Now send the line, compressing zero pixels
uint16_t zeroes = 0;
for (xx = 0; xx < 800; xx += 2) {
uint16_t v = GD.rd16(SCREENSHOT + xx);
if (v == 0) {
zeroes++;
} else {
if (zeroes) {
Serial.write(lowByte(zeroes));
Serial.write(0x80 | highByte(zeroes));
zeroes = 0;
}
Serial.write(lowByte(v));
Serial.write(highByte(v));
}
}
if (zeroes) {
Serial.write(lowByte(zeroes));
Serial.write(0x80 | highByte(zeroes));
}
}
GD.wr16(SCREENSHOT_Y, 0); // restore screen to normal
}
void AX12::setMultiPosVel (byte targetlength, byte* targets, int* posvalues, int* velvalues) {
byte valuess [targetlength][4];
byte * pointers [targetlength];
for (byte f=0; f<targetlength; f++) {
valuess [f][0] = lowByte(posvalues[f]);
valuess[f][1] = highByte(posvalues[f]);
valuess [f][2] = lowByte(velvalues[f]);
valuess[f][3] = highByte(velvalues[f]);
pointers[f] = &valuess[f][0];
}
//nota: la sync write no respeta la propiedad "inverse"
syncWrite (GOAL_POSITION, 4, targetlength, targets, pointers);
}
void BnrOneA::setPID(int kp, int ki, int kd)
{
byte kp_H=highByte(kp);
byte kp_L=lowByte(kp);
byte ki_H=highByte(ki);
byte ki_L=lowByte(ki);
byte kd_H=highByte(kd);
byte kd_L=lowByte(kd);
byte buffer[]={KEY1,KEY2,kp_H,kp_L,ki_H,ki_L,kd_H,kd_L};
spiSendData(COMMAND_PID_CFG,buffer,sizeof(buffer));
delay(20);//Wait while command is processed
}
void PCA9685::writeLED(int ledNumber, word LED_ON, word LED_OFF) { // LED_ON and LED_OFF are 12bit values (0-4095); ledNumber is 0-15
if (ledNumber >=0 && ledNumber <= 15) {
Wire.beginTransmission(_i2cAddress);
Wire.write(PCA9685_LED0 + 4*ledNumber);
Wire.write(lowByte(LED_ON));
Wire.write(highByte(LED_ON));
Wire.write(lowByte(LED_OFF));
Wire.write(highByte(LED_OFF));
Wire.endTransmission();
}
}
void ATEM::changeColorValue(uint8_t colorGenerator, uint16_t hue, uint16_t saturation, uint16_t lightness) {
if (colorGenerator>=1 && colorGenerator<=2
&& hue>=0 && hue<=3600
&& saturation >=0 && saturation <=1000
&& lightness >=0 && lightness <= 1000
) { // Todo: Should match available aux outputs
uint8_t commandBytes[8] = {0x07, colorGenerator-1,
highByte(hue), lowByte(hue),
highByte(saturation), lowByte(saturation),
highByte(lightness), lowByte(lightness)
};
_sendCommandPacket("CClV", commandBytes, 8);
}
}
void Trex::masterSend(int _lmspeed, byte _lmbrake, int _rmspeed, byte _rmbrake)
{
//================================================================ Send Data ==============================================================
Wire.beginTransmission(I2Caddress); // transmit data to 7
Wire.write(startbyte); // start byte
Wire.write(pfreq); // pwm frequency
Wire.write(highByte(_lmspeed)); // MSB left motor speed
Wire.write( lowByte(_lmspeed)); // LSB left motor speed
Wire.write(_lmbrake); // left motor brake
Wire.write(highByte(_rmspeed)); // MSB right motor speed
Wire.write( lowByte(_rmspeed)); // LSB right motor speed
Wire.write(_rmbrake); // right motor brake
Wire.write(highByte(sv[0])); // MSB servo 0
Wire.write( lowByte(sv[0])); // LSB servo 0
Wire.write(highByte(sv[1])); // MSB servo 1
Wire.write( lowByte(sv[1])); // LSB servo 1
Wire.write(highByte(sv[2])); // MSB servo 2
Wire.write( lowByte(sv[2])); // LSB servo 2
Wire.write(highByte(sv[3])); // MSB servo 3
Wire.write( lowByte(sv[3])); // LSB servo 3
Wire.write(highByte(sv[4])); // MSB servo 4
Wire.write( lowByte(sv[4])); // LSB servo 4
Wire.write(highByte(sv[5])); // MSB servo 5
Wire.write( lowByte(sv[5])); // LSB servo 5
Wire.write(devibrate); // devibrate
Wire.write(highByte(sensitivity)); // MSB impact sensitivity
Wire.write( lowByte(sensitivity)); // LSB impact sensitivity
Wire.write(highByte(lowbat)); // MSB low battery voltage 550 to 30000 = 5.5V to 30V
Wire.write( lowByte(lowbat)); // LSB low battery voltage
Wire.write(i2caddr); // I2C slave address for T'REX controller
Wire.write(i2cfreq); // I2C clock frequency: 0=100kHz 1=400kHz
Wire.endTransmission(); // stop transmitting
delay(50);
masterReceive();
}
/**
* Send 16-bit unsigned integer to computer through serial port.
* Function to send a raw unsigned 16-bit integer across the Serial
* interface in network byte order (i.e., big endian). Serial
* interface must be initialized with Serial.begin() prior to
* function call. More than likely you will be reading this
* integer with a corresponding libuino.h "read" function
* on the attached computer.
* @param n 16-bit unsigned integer to send.
* @since 0.1
*/
void ino_send_uint16(uint16_t n)
{
ino_int16 i;
i.unsig = n;
Serial.write(highByte(i.unsig));
Serial.write(lowByte(i.unsig));
}
// write 2 bytes
void vis_gd_wr16(unsigned int addr, unsigned int v)
{
vis_gd_wstart(addr);
vis_spi_transfer(lowByte(v));
vis_spi_transfer(highByte(v));
vis_gd_end();
}
void RegisterList::setFunction(char *s) volatile{
byte b[5]; // save space for checksum byte
int cab;
int fByte, eByte;
int nParams;
byte nB=0;
nParams=sscanf(s,"%d %d %d",&cab,&fByte,&eByte);
if(nParams<2)
return;
if(cab>127)
b[nB++]=highByte(cab) | 0xC0; // convert train number into a two-byte address
b[nB++]=lowByte(cab);
if(nParams==2){ // this is a request for functions FL,F1-F12
b[nB++]=(fByte | 0x80) & 0xBF; // for safety this guarantees that first nibble of function byte will always be of binary form 10XX which should always be the case for FL,F1-F12
} else { // this is a request for functions F13-F28
b[nB++]=(fByte | 0xDE) & 0xDF; // for safety this guarantees that first byte will either be 0xDE (for F13-F20) or 0xDF (for F21-F28)
b[nB++]=eByte;
}
loadPacket(0,b,nB,4,1);
} // RegisterList::setFunction()
void IoesptPersistance::saveSettings(SaveCallbackType callback)
{
EEPROM.begin(BufferLen + 2);
StaticJsonBuffer<2000> jsonBuffer;
JsonObject& root = jsonBuffer.createObject();
callback(root);
int bufferLength = root.printTo(buffer, length);
DEBUG_WMS("Saving Eprom contents length:");
DEBUG_WMF(length);
#ifdef IOESPTPERSISTENCE_VERBOSE_OUT
root.prettyPrintTo(Serial);
#endif // IOESPTPERSISTENCE_VERBOSE_OUT
DEBUG_WMF("");
EEPROM.write(0, highByte(bufferLength));
EEPROM.write(1, lowByte(bufferLength));
for (int address = 2; address < bufferLength + 2; address++) {
EEPROM.write(address, buffer[address - 2]);
}
EEPROM.commit();
}
unsigned char TxPacket(unsigned char *data,unsigned char length)
{
#define CHECK_TXD_FINISH tx_counter<=0
int i;
unsigned int checksum;
tx_buffer[0]=0xFF;
tx_buffer[1]=0xFF;
tx_buffer[2]=length+3; //1 byte ID, 2 byte chksum
checksum=tx_buffer[2];
for(i=0;i<length;i++)
{
tx_buffer[i+3]=*(data+i);
checksum+=*(data+i);
}
//checksum+=1;
checksum=0xffff-checksum;
tx_buffer[i+3]=highByte(checksum);
tx_buffer[i+4]=lowByte(checksum);
//tx_buffer hoan chinh, chuan bi truyen di
tx_rd_index=1;
tx_counter=length+4;
UDR=tx_buffer[0];
while(!CHECK_TXD_FINISH);
if (RxPacket(255)==0xff || error_code!=OK) return 0;
else return 1;
}
void DSP0801Class::display() {
for (int i = 0; i < MAX_CURSOR; i++) {
shiftOut(_MOSI, _CLK, MSBFIRST, highByte(data_array[i]));
shiftOut(_MOSI, _CLK, MSBFIRST, lowByte(data_array[i]));
}
pulseLAT();
}
EZPacket* CMUCAM::createAbstractionChunkPacket(byte nodeid, byte datareceiver) {
int i;
uint16_t paramlen;
// Creating an EZPacket
EZPacket* pack = (EZPacket*)malloc(sizeof(EZPacket));
pack->SenderID = nodeid;
pack->ReceiverID = datareceiver;
pack->SenderNodeType = t_Node_Robot;
// Message length should be 4 bytes (command) + CHUNKSIZE (or remaining bytes) ()
pack->MessageLength = 4+RawbytesLength;
// allocating the message
pack->Message = (byte*)malloc(pack->MessageLength);
// now filling the message
pack->Message[0] = (byte)t_RobosapienV2; // robot type
pack->Message[1] = (byte)rc_CMUCAMAbstractionData; // command code
paramlen = RawbytesLength;
pack->Message[2] = lowByte(paramlen); // low byte of the command's parameters length
pack->Message[3] = highByte(paramlen); // high byte of the length of the parameters
Serial.print("Parameter Length: ");
Serial.println(paramlen, DEC);
// copying the bytes now
for (i=0; i<paramlen; i++)
pack->Message[4+i] = RawbytesBuffer[i];
return pack;
}
void Potar::writeEeprom(){
if(flags == 1){
if((millis()-millisflag)>600000){
byte hiByteMin = highByte(min);
byte loByteMin = lowByte(min);
byte hiByteMax = highByte(max);
byte loByteMax = lowByte(max);
EEPROM.write((4*in),hiByteMin);
EEPROM.write((4*in)+1,loByteMin);
EEPROM.write((4*in)+2,hiByteMax);
EEPROM.write((4*in)+3,loByteMax);
flags = 0;
}
}
}
