// set up velocity profile structure - assume 4 set points for now, generalize later
void cmdSetVelProfile(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){
int interval[NUM_VELS], delta[NUM_VELS], vel[NUM_VELS];
int idx = 0, i = 0;
for(i = 0; i < NUM_VELS; i ++)
{ interval[i] = frame[idx]+ (frame[idx+1]<<8);
idx+=2; }
for(i = 0; i < NUM_VELS; i ++)
{ delta[i] = frame[idx]+ (frame[idx+1]<<8);
idx+=2; }
for(i = 0; i < NUM_VELS; i ++)
{ vel[i] = frame[idx]+ (frame[idx+1]<<8);
idx+=2; }
setPIDVelProfile(0, interval, delta, vel);
for(i = 0; i < NUM_VELS; i ++)
{ interval[i] = frame[idx]+ (frame[idx+1]<<8);
idx+=2; }
for(i = 0; i < NUM_VELS; i ++)
{ delta[i] = frame[idx]+ (frame[idx+1]<<8);
idx+=2; }
for(i = 0; i < NUM_VELS; i ++)
{ vel[i] = frame[idx]+ (frame[idx+1]<<8);
idx+=2; }
setPIDVelProfile(1, interval, delta, vel);
//Send confirmation packet
radioConfirmationPacket(RADIO_DEST_ADDR, CMD_SET_VEL_PROFILE, status, 48, frame);
return; //success
}
unsigned char cmdSetVelProfile(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) {
int interval[NUM_VELS], delta[NUM_VELS], vel[NUM_VELS], period, onceFlag;
int idx = 0, i = 0;
// Packet structure [Period, delta[NUM_VELS], FLAG, Period, delta[NUM_VELS], FLAG]
period = frame[idx] + (frame[idx + 1]<<8);
idx+=2;
for(i = 0; i < NUM_VELS; i ++) {
interval[i] = period/NUM_VELS;
delta[i] = (frame[idx]+ (frame[idx+1]<<8));
if(delta[i]>=8192){
delta[i] = 8191;
} else if(delta[i] < -8192){
delta[i] = -8192;
}
delta[i] = delta[i]<<2;
vel[i] = delta[i]/interval[i];
idx+=2;
}
onceFlag = frame[idx] + (frame[idx + 1]<<8);
idx+=2;
setPIDVelProfile(0, interval, delta, vel, onceFlag);
period = frame[idx] + (frame[idx + 1]<<8);
idx+=2;
for(i = 0; i < NUM_VELS; i ++) {
interval[i] = period/NUM_VELS;
delta[i] = (frame[idx]+ (frame[idx+1]<<8));
if(delta[i]>=8192){
delta[i] = 8191;
} else if(delta[i] < -8192){
delta[i] = -8192;
}
delta[i] = delta[i]<<2;
vel[i] = delta[i]/interval[i];
idx+=2;
}
onceFlag = frame[idx] + (frame[idx + 1]<<8);
setPIDVelProfile(1, interval, delta, vel, onceFlag);
//Send confirmation packet
// TODO : Send confirmation packet with packet index
return 1; //success
}
int setLegFreqs(int numfreqs, int* freq) {
//freq is in milli-Hz
int i, j;
int interval[NUM_PIDS][NUM_VELS], delta[NUM_PIDS][NUM_VELS], vel[NUM_PIDS][NUM_VELS], period[NUM_PIDS];
int setdelta = 0x4000;
int onceFlag = 0;
for (i = 0; i < numfreqs; i++) {
//upper bound
int upperbound = 8000; //don't want to go above 8Hz
if (freq[i] > upperbound) {
freq[i] = upperbound;
} else if (freq[i] < -upperbound) {
freq[i] = -upperbound;
} else if (freq[i] < 31 && freq[i] > -31) { //min value 31 because 1000000/30 > 2^15
if (freq[i] >= 0) {
freq[i] = 31;
} else {
freq[i] = -31;
}
}
period[i] = 1000000/freq[i]; //convert to ms
for (j = 0; j < NUM_VELS; j++) {
interval[i][j] = period[i]/NUM_VELS;
delta[i][j] = setdelta;
vel[i][j] = ((long)delta[i][j])*freq[i]*NUM_VELS/1000000;
if (period[i] < 0) {
//interval must always be positive
//flip delta if negative frequency
//vel will already be negative
interval[i][j] = -interval[i][j];
delta[i][j] = -delta[i][j];
}
}
setPIDVelProfile(i, interval[i], delta[i], vel[i], onceFlag);
}
return 0;
}
