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