/* abot_rbasic_bt_01.c for overlay board Use with robotbasic "abot_bt_01.bas" Commands Activitybot to move straight, pivot 45 deg, move in a curve, increase or decrease speed and curve radius, and stop. Also has whisker code for obstacle avoidance. Use multiple cogs - new cog to handle whiskers There is no delay between whisker touching and bot stopping. This version stops, backs up, turns 90 deg from obstacle and stops. Has ramping to speed. Increment = 4 ticks per 1/50 sec (4/0.020 sec). To get from 0 to 100 takes 25 increments = 0.5 sec Load EEProm and Run. When abot beeps start the RobotBasic program. */ #include "simpletools.h" // Libraries included #include "fdserial.h" #include "abdrive.h" // abdrive library // ***** Remote control aliases -- uses codes defined in "abot_bt_01.bas" ***** #define fwd 1 // -- forward (use ispd and dspd to change speed) #define stp 16 // -- stop #define rvs 4 // -- reverse (speed set to -spd0) #define pl 8 // -- pivot left 45 deg #define pr 2 // -- pivot right #define cvl 64 // -- curve left (faster base speed = wider radius) #define cvr 32 // -- curve right #define ispd 34 // -- increase speed #define dspd 35 // -- decrease speed // ***** BT defines ***** #define rxpin 6 // BT out --> Prop in #define txpin 5 // BT in <-- Prop out #define btbaud 115200 // set depending on BT device speed // ***** end BTdefines ***** // ***** Pin Defines ***** #define spkrpin 3 // piezo speaker pin for overlay board #define Rwpin 8 // Right whisker pin -- must be 1 pin number higher than Lwpin #define Lwpin 7 // Left whisker pin // ***** Function Prototype ***** void whisker(); // monitors whiskers, if hit: stops, backs away, turns away, stops. Run in separate cog // ***** Global defines **** int *cogwhisker; // pointer to cog ID int Lspd, Rspd = 0; // Left and Right command wheel speeds, start at 0 volatile char c1; volatile int bumpflg; // 0 = remote has control. 1 = whisker hit, whisker code has control. int button, buttonflg; // value from remote, flg = 1 if command received int spd0 = 20; // intial fwd speed int spd; // current base speed int spdmax = 120; int spdmin = -20; unsigned char Lspdbyte, Rspdbyte; // wheel speed data to send to Robot Basic program fdserial *blut; // declare bluetooth as serial device // ***** main function ***** int main() { spd = spd0; // initial speed once fwd command given int dflg = 0; // flag: -1=fwd or rev, 0=stopped, 1=curve left, 2=curve right, 3=pivot L or R drive_speed(0, 0); // start program wheels stopped bumpflg = 0; // Gives control to remote cogwhisker = cog_run(whisker, 100); // start monitoring whiskers blut = fdserial_open(rxpin, txpin, 0, btbaud); // start fdserial freqout(spkrpin,1000, 3000); // Speaker tone: 1 s, 3 kHz while(c1 != 255) // Repeat until RB program sends "end" command (255) { c1 = fdserial_rxChar(blut); // read byte from blut button = c1; buttonflg = 1; switch(button) // decode signal and calc drive command { case stp : // stop Lspd = 0; Rspd = 0; dflg = 0; spd = spd0; break; case fwd : // forward -- start moving forward at speed spd Lspd = spd; Rspd = spd; dflg = -1; break; case rvs : // reverse -- move in reverse at speed spd0 spd = spd0; Lspd = -spd; Rspd = -spd; dflg = -1; break; case pr : // Pivot Right 45 deg & stop Lspd = 13; Rspd = -13; dflg = 3; break; case pl : // Pivot Left 45 deg & stop Lspd = -13; Rspd = 13; dflg = 3; break; case cvr : // Curve Right -- right wheel speed reduced by 20 from current speed if (spd<25) spd=25; // minimum speed must be 5 more than amount rt wheel speed reduced Lspd = spd; // Increase speed (ispd) maked radius larger Rspd = spd-20; // decrease speed (dspd) makes radius tighter dflg = 2; // press "fwd" to go straight break; case cvl : // Curve Left -- left wheel speed reduced by 20 from current speed if (spd<25) spd=25; Lspd = spd-20; Rspd = spd; dflg = 1; break; case ispd : // Increase speed if (dflg == 1) // if curving left - increase spd and move in curve at new spd { spd=spd+5; if (spd > spdmax) spd = spdmax; Lspd = spd-20; Rspd = spd; } else if (dflg == 2) // if curving right - increase spd and move in curve at new spd { spd=spd+5; if (spd > spdmax) spd = spdmax; Lspd = spd; Rspd = spd-20; } else if (dflg == -1) // if straight - increase spd and move at new spd { spd=spd+5; if (spd > spdmax) spd = spdmax; Lspd = spd; Rspd = spd; } else { spd=spd+5; if (spd > spdmax) spd = spdmax;; } break; // Finish increase speed case dspd : // Decrease spd if (dflg == 1) // if curving left - decrease spd and move in curve at new spd { spd=spd-5; if (spd < 25) spd = 25; Lspd = spd-20; Rspd = spd; } else if (dflg == 2) // if curving right - decrease spd and move in curve at new spd { spd=spd-5; if (spd < 25) spd = 25; Lspd = spd; Rspd = spd-20; } else if (dflg == -1) // if straight - decrease spd and move at new spd { spd=spd-5; if (spd < spdmin) spd = spdmin; Lspd = spd; Rspd = spd; } else // stopped { spd=spd-5; if (spd < spdmin) spd = spdmin; } break; // Finish Decrease Speed default : buttonflg = 0; break; } // end switch if(dflg == 3) // execute pivot { if( ! bumpflg) drive_ramp(0,0); // stop for pivot, whiskers not controlling movement if( ! bumpflg) // whiskers not controlling movement { drive_ramp(Lspd, Rspd); // now pivot 45 deg pause(1000); Lspd = 0; // and stop Rspd = 0; dflg = 0; } } // end execute pivot // **** Execute other drive commands if whisker() not controlling movement **** if( ! bumpflg) drive_ramp(Lspd, Rspd); // execute drive comnd // ***** If BT command received, send wheel speeds back to RobotBasic program if(buttonflg) { Lspdbyte = Lspd + 120; // make positive for transmitting to RobotBasic program Rspdbyte = Rspd + 120; printi("%d %d\n", Lspdbyte-120, Rspdbyte-120); // for debugging fdserial_txChar(blut, Lspdbyte); // send to RB program fdserial_txChar(blut, Rspdbyte); buttonflg = 0; } pause(10); } // end while. "end program" command received drive_ramp(0,0); // stop bot freqout(spkrpin,500,2000); // Speaker tone: 0.5 s, 2 kHz } // end main // ***** Function whisker ********************************************************* // If whisker pressed, stop & move away from obstacle: // 1. stop bot, 2. disable remote control, 3. back up, 4. turn away from obstacle, // 5. stop, 6. restore remote control // Uses waitpne(wstate, wmask) - cog sleeps until whisker pressed // Rwpin must be 1 pin higher than Lwpin // ********************************************************************************* void whisker() { int wmask = 1 << Rwpin | 1 << Lwpin; // build 32 bit int mask with whisker pin numbers int wstate = wmask; // normal running both pins are high, hit whisker pin drops to low int RLval; while(1) // runs continuously { waitpne(wstate, wmask); //stop cog until a whisker is pressed RLval = get_states(Rwpin, Lwpin); // get whisker pin values bumpflg = 1; // Set flag so main will not give conflicting drive commands drive_speed(0,0); // stop drive_ramp(-60, -60); // Back up 0.5 seconds pause(500); drive_ramp(0,0); // and stop // ***** Check direction to turn ***** switch(RLval) // decode whiskers 0b00=both, 0b01=right, 0b10=left, 0b11=neither { case 0 : // *** both whiskers hit, turn right drive_speed(26, -26); // Turn right 1 seconds -- 90 deg break; case 1 : // *** Right whisker hit, turn left drive_speed(-26, 26); // Turn left 1 seconds -- 90 deg break; case 2 : // *** Left whisker hit, turn right drive_speed(26, -26); // Turn right 1 seconds -- 90 deg break; case 3 : // *** Neither whisker hit, don't turn break; } // end switch pause(1000); // delay 1 second for the 90 degree turn drive_ramp(0, 0); // and stop Lspd = 0; // prevents bot from resuming last movement command in main Rspd = 0; spd = spd0; pause(10); bumpflg = 0; // clear flag, allow main to have control } // end while } // end whisker function