//so long as we are connected to the controller, no reason to use the direct motor command instead of this one
void create_motors(int speed)//speed is from 0 to 128 inclusive
{
create_write_byte(144);
create_write_byte(speed);
create_write_byte(speed);
create_write_byte(speed);
}
//This function sets the three digital out put pins 20,7,19 where 20 is the high bit and 19 is the low. bits should have a value 0 to 7.
void create_digital_output(int bits)
{
CREATE_BUSY;
create_write_byte(147);
create_write_byte(bits);
CREATE_FREE;
}
void create_special_connect(){
serial_init();
create_write_byte(128);
create_write_byte(132);
create_power_led(250,254);
atexit(create_disconnect);
}
// updates the cliff sensor globals with the current sensor values from the Create
int create_cliffs() {
char buffer[12];
char *bptr = buffer;
CREATE_BUSY;
create_write_byte(149);
create_write_byte(8); // send 8 packets
create_write_byte(9); // cliff sensor detected or not (single byte packets)
create_write_byte(10);
create_write_byte(11);
create_write_byte(12);
create_write_byte(28); // cliff sensor signal strength (two byte packets)
create_write_byte(29);
create_write_byte(30);
create_write_byte(31);
CREATE_WAITFORBUFFER(buffer,12,-1)
gc_lcliff=(*(bptr++));
gc_lfcliff=(*(bptr++));
gc_rfcliff=(*(bptr++));
gc_rcliff=(*(bptr++));
gc_lcliff_amt=256*(*(bptr++));
gc_lcliff_amt=gc_lcliff_amt + (*(bptr++));
gc_lfcliff_amt=256*(*(bptr++));
gc_lfcliff_amt=gc_lfcliff_amt + (*(bptr++));
gc_rfcliff_amt=256*(*(bptr++));
gc_rfcliff_amt=gc_rfcliff_amt + (*(bptr++));
gc_rcliff_amt=256*(*(bptr++));
gc_rcliff_amt=gc_rcliff_amt + (*(bptr++));
CREATE_FREE;
return(0);
}
//Turns on and off the signal for the three low side drivers (128 = 100%). pwm2 is pin 24, pwm1 pin 23 and pwm0 pin 22
//A 0 or 1 should be given for each of the drivers to turn them off or on.
void create_low_side_drivers(int pwm2, int pwm1, int pwm0)
{
CREATE_BUSY;
create_write_byte(138);
create_write_byte(4*pwm2+2*pwm1+pwm0);
CREATE_FREE;
}
//Sets the PWM signal for the three low side drivers (128 = 100%). pwm2 is pin 24, pwm1 pin 23 and pwm0 pin 22
void create_pwm_low_side_drivers(int pwm2, int pwm1, int pwm0)
{
CREATE_BUSY;
create_write_byte(144);
create_write_byte(pwm2);
create_write_byte(pwm1);
create_write_byte(pwm0);
CREATE_FREE;
}
// This command drives the robot along a curve with radius (in mm) radius; and at a speed (mm/sec) of speed
// a radius of 32767 will drive the robot straight
// a radius of 1 will spin the robot CCW
// a radius of -1 will spin the robot CW
// Negative radii will be right turns, positive radii left turns
void create_drive (int speed, int radius)
{
CREATE_BUSY;
create_write_byte(137);
create_write_byte(HIGH_BYTE(speed));
create_write_byte(LOW_BYTE(speed));
create_write_byte(HIGH_BYTE(radius));
create_write_byte(LOW_BYTE(radius));
CREATE_FREE;
}
// this function drives the right wheel at r_speed and the left wheel at l_speed
// speeds for all of these functions are +/-500mm/sec.
void create_drive_direct(int r_speed, int l_speed)
{
CREATE_BUSY;
create_write_byte(145);
create_write_byte(HIGH_BYTE(r_speed));
create_write_byte(LOW_BYTE(r_speed));
create_write_byte(HIGH_BYTE(l_speed));
create_write_byte(LOW_BYTE(l_speed));
CREATE_FREE;
}
// special version of command above drives robot at speed 0, stopping it.
void create_stop()
{
CREATE_BUSY;
create_write_byte(137);
create_write_byte(0);
create_write_byte(0);
create_write_byte(0);
create_write_byte(0);
CREATE_FREE;
}
// special version of command spins robot CCW with the wheels turning at speed speed
void create_spin_CCW (int speed)
{
CREATE_BUSY;
create_write_byte(137);
create_write_byte(HIGH_BYTE(speed));
create_write_byte(LOW_BYTE(speed));
create_write_byte(HIGH_BYTE(1));
create_write_byte(LOW_BYTE(1));
CREATE_FREE;
}
// special version of command above drives robot at speed speed. Negative speed will drive robot backwards
void create_drive_straight (int speed)
{
CREATE_BUSY;
create_write_byte(137);
create_write_byte(HIGH_BYTE(speed));
create_write_byte(LOW_BYTE(speed));
create_write_byte(HIGH_BYTE(32767));
create_write_byte(LOW_BYTE(32767));
CREATE_FREE;
}
//updates values for charging and battery state
int create_battery_charge()
{
char buffer[11];
char *bptr = buffer;
CREATE_BUSY;
create_write_byte(149);
create_write_byte(7);
create_write_byte(21);
create_write_byte(22);
create_write_byte(23);
create_write_byte(24);
create_write_byte(25);
create_write_byte(26);
create_write_byte(34);
CREATE_WAITFORBUFFER(buffer,11,-11)
gc_charge_state=(*(bptr++));
gc_batt_voltage=(*(bptr++))*256;
gc_batt_voltage=gc_batt_voltage+(*(bptr++));
gc_current_flow=(*(bptr++))*256;
gc_current_flow=gc_current_flow+(*(bptr++));
if(gc_current_flow>32767)gc_current_flow=gc_current_flow-65536;
gc_batt_temp==(*(bptr++));
if(gc_batt_temp>127)gc_batt_temp=gc_batt_temp-128;
gc_batt_charge=(*(bptr++))*256;
gc_batt_charge=gc_batt_charge+(*(bptr++));
gc_batt_capacity=(*(bptr++))*256;
gc_batt_capacity=gc_batt_capacity+(*(bptr++));
gc_charge_source=(*(bptr++));//1 if charger 2 if home base 0 if no charge source
CREATE_FREE;
return(0);
}
//sets gc_overcurrents to have a state where 16s'bit is left wheel; 8's is right wheel, 4's id LD2, 2's is LD0 and 1's is LD1
int create_motor_overcurrents(){
char buffer[1];
char *bptr = buffer;
CREATE_BUSY;
create_write_byte(142);
create_write_byte(14);
CREATE_WAITFORBUFFER(buffer,1,-10)
gc_overcurrents=(*(bptr++));
CREATE_FREE;
return(0);
}
//sets create power led. Color =0 is green and color = 255 is red -- with intermediate colors
//brightness of 0 is off and 255 is fully bright
void create_power_led(int color, int brightness)
{
gc_leds[1]=color;
gc_leds[2]=brightness;
CREATE_BUSY;
create_write_byte(139);
create_write_byte(gc_leds[0]);
create_write_byte(gc_leds[1]);
create_write_byte(gc_leds[2]);
CREATE_FREE;
}
// gc_IR=255 indicates no IR byte is being received
int create_read_IR() {
char buffer[1];
char *bptr = buffer;
CREATE_BUSY;
create_write_byte(142);
create_write_byte(17); // IR packet
CREATE_WAITFORBUFFER(buffer,1,-1)
gc_IR=(*(bptr++));
CREATE_FREE;
return(0);
}
// see the roomba SCI manual for note codes.
void create_play_song(int num)
{
int i;
if(num >= 0 && num <=15 ){
CREATE_BUSY;
create_write_byte(141);
create_write_byte(num);
CREATE_FREE;
}
else printf("Song array reference is out of bounds\n");
}
// Create open interface mode is off=0 (Create powered), passive=1, safe=2, full=3
// From "safe" mode, wheel drop or cliff detect halts motors and puts Create in passive mode
int create_mode()
{
char buffer[1];
char *bptr = buffer;
CREATE_BUSY;
create_write_byte(142);
create_write_byte(35);
CREATE_WAITFORBUFFER(buffer,1,-1)
gc_mode=(*(bptr++));
CREATE_FREE;
return(0);
}
// updates the bumper and wheel drop globals with the current values from the Create
int create_buttons() {
char buffer[1];
char *bptr = buffer;
int b;
CREATE_BUSY;
create_write_byte(142);
create_write_byte(18);//buttons
CREATE_WAITFORBUFFER(buffer,1,-1)
b=(*(bptr++));
CREATE_FREE;
gc_play_button=b & 0x1;
gc_advance_button=(b >> 2) & 0x1;
return(0);
}
// turns on and off the advance (>>) LED (0 is off, 1 is on)
void create_advance_led(int on)
{
if(on){
if(!(gc_leds[0] & 8)) gc_leds[0]=gc_leds[0]+ 8;
}
else{
if(gc_leds[0] & 8) gc_leds[0]=gc_leds[0]- 8;
}
CREATE_BUSY;
create_write_byte(139);
create_write_byte(gc_leds[0]);
create_write_byte(gc_leds[1]);
create_write_byte(gc_leds[2]);
CREATE_FREE;
}
// turns on and off the play LED (0 is off, 1 is on)
void create_play_led(int on)
{
if(on){
if(!(gc_leds[0] & 2)) gc_leds[0]=gc_leds[0]+ 2;
}
else{
if(gc_leds[0] & 2) gc_leds[0]=gc_leds[0]- 2;
}
CREATE_BUSY;
create_write_byte(139);
create_write_byte(gc_leds[0]);
create_write_byte(gc_leds[1]);
create_write_byte(gc_leds[2]);
CREATE_FREE;
}
// this loads a song into the robot's memory.
// song can be numbered 0 to 15. the first element in each row of the array should be
// the number of notes (1-16) the subsequent pairs of bytes should be tone and duration
// see the roomba SCI manual for note codes.
// user's program should load the song data into the array before calling this routine
void create_load_song(int num)
{
int i, numnotes;
numnotes=gc_song_array[num][0];
if(num >= 0 && num <=15 && numnotes > 0 && numnotes <= 16){
CREATE_BUSY;
create_write_byte(140);
create_write_byte(num);
create_write_byte(numnotes);
for(i=1; i< 2*numnotes+1; i++)
create_write_byte(gc_song_array[num][i]);
CREATE_FREE;
}
else printf("illegal song #%d is %d notes long being written to memory\n", num, gc_song_array[num][0]);
}
void create_backward(int dist, int speed)
{
create_write_byte(145);
create_write_int(-speed);
create_write_int(-speed);
create_wait_dist(-dist);
}
void create_forward(int dist, int speed)
{
create_write_byte(145);
create_write_int(speed);
create_write_int(speed);
create_wait_dist(dist);
}
void create_turn_left(int angle, int radius, int speed)
{
create_write_byte(137);
create_write_int(speed);
create_write_int(radius);
create_wait_angle(angle);
}
void create_turn_right(int angle, int radius, int speed)
{
create_write_byte(137);
create_write_int(speed);
create_write_int(-radius);
create_wait_angle(-angle);
}
void create_drive_direct_right(int r_speed, int l_speed, int angle)
{
create_write_byte(145);
create_write_int(r_speed);
create_write_int(l_speed);
create_wait_angle(-angle);
}
void create_drive_direct_dist(int r_speed, int l_speed, int dist)
{
create_write_byte(145);
create_write_int(r_speed);
create_write_int(l_speed);
create_wait_dist(dist);
}
//This function blocks and does a pretty accurate spin. Note that the function will
//not return until the spin is complete
//CAUTION, requesting the robot to spin more than about 3600 degrees may never terminate
//the 9/07 firmware update is required for this works. Returns -1 if error
int create_spin_block(int speed, int angle)
{
char buffer[7];
char *bptr = buffer;
int r[7]={18,7,9,24,12,15,21},v[7],i,b;
long lenc=0L, renc=0L,slenc,srenc,flenc,frenc;
float offset, ticsPerMM=7.8324283, rad=129.0, pi=3.1415926;
CREATE_BUSY;
create_write_byte(8);//version
CREATE_WAITFORBUFFER(buffer,7,-99)
for(i=0;i<7;i++){
v[i]=(*(bptr++));
}
CREATE_FREE;
if(!(v[1]==r[1] && v[2]==r[2] && v[3]==r[3])){
printf("create_spin_block\n requires Create Firmware dated\n 9/24/2007\n");
printf("Yours is:%d/%d/%d\nFunction aborted\n", v[2], v[3], v[1]+2000);
return(-1);
}
_create_get_raw_encoders(&slenc, &srenc);
offset=(float)angle*pi*rad*ticsPerMM/180.0;
if(angle>0){create_drive_direct(speed,-speed);}
else {offset=-offset; create_drive_direct(-speed,speed);}
flenc=slenc+(long)offset;
frenc=srenc+(long)offset;
if(frenc > 65535L && flenc > 65000L){
while(lenc >= slenc)_create_get_raw_encoders(&lenc, &renc);
while(lenc < flenc-65536L)_create_get_raw_encoders(&lenc, &renc);
create_stop();}
else{
while(lenc < flenc && renc < frenc)_create_get_raw_encoders(&lenc, &renc);
create_stop();
}
return(0);
}
void create_backward_fast(int dist,int speed)
{
create_write_byte(145);
create_write_int(-speed*lSpeedMult/100);
create_write_int(-speed*rSpeedMult/100);
create_wait_dist(-dist);
}
int _create_get_raw_encoders(long *lenc, long *renc)
{
char buffer[4];
char *bptr = buffer;
CREATE_BUSY;
create_write_byte(149);
create_write_byte(2);
create_write_byte(43);
create_write_byte(44);
CREATE_WAITFORBUFFER(buffer,4,-14)
*lenc=(long)(*(bptr++))*256L;
*lenc=*lenc+(long)(*(bptr++));//left encoder value
*renc=(long)(*(bptr++))*256L;
*renc=*renc+(long)(*(bptr++));//left encoder value
CREATE_FREE;
}
