int main(void)
{
/*System time clock configuartion*/
SysTick_INIT ();
/*Configuration StepDirver GPIO*/
StepDirverSetup();
/*PWM out put Configuartion for GPIO A and GPIO C use time2 time3*/
RCC_Configuration();
TIM_PWMConfig();
/*USART Configuartion After initial success output " Hello Word !\r\n" if */
usart_init();
int CMD_VAL; // save CMD numeric part
CMD_VAL=255;
//test LED config
MOS_Out(1, CMD_VAL);
MOS_Out(2, CMD_VAL);
MOS_Out(3, CMD_VAL);
while(1)
{
if (USART_CMDFLAG == 1) // wait for FLAG =1
{
USART_CMDFLAG = 0; // Reset FLAG
CMD_VAL = (CMD_DATABUFF[3] - 48) * 100 + (CMD_DATABUFF[4] - 48) * 10 + (CMD_DATABUFF[5] - 48);
if (CMD_DATABUFF[0] == 'S')
if (CMD_DATABUFF[1] == ',')
{
if (CMD_DATABUFF[2] == '+')
{
rotateDeg(CMD_VAL);
}
if (CMD_DATABUFF[2] == '-')
{
rotateDeg(0 - CMD_VAL);
}
}
if (CMD_DATABUFF[0] == 'M'){ // if OBJ = M set LED brightness
MOS_Out(1, CMD_VAL);
MOS_Out(2, CMD_VAL);
MOS_Out(3, CMD_VAL);
}
if (CMD_DATABUFF[0] == 'G') // if OBJ = G set Glass Transparency
CARD_Out(1, CMD_VAL);
}
}
}
char aggressiveKid(short angle, float front, float side)
{
// Returns a char if buffer distance is broken.
// If buffer not broken, it returns 'w'
// after randomly moving a short distance
getIR(); // Reads sensors
if (lt <= side) // Checks left buffer
{
SPKR_play_beep(250, 250, 100); // Beep to indicate end
return 'l';
}
else if (rt <= side) // Checks right buffer
{
SPKR_play_beep(250, 250, 100); // Beep to indicate end
return 'r';
}
else if (ft <= front) // Checks front buffer
{
SPKR_play_beep(250, 250, 100); // Beep to indicate end
return 'f';
}
else
{
// Calculate very small random angle increment
if(angle==1)
{
int angle = 21-randRange(1,40);
rotateDeg(angle); // Rotates
}
forward(move); // Moves forward
return 'w';
}
}
void loop() {
if (stringComplete) {
for (int i = 0; i < inputString.length(); i++) {
if (inputString[i] == 'B') {
rotateDeg(B_DIR_PIN, B_STEP_PIN, DEGREE_FACTOR, 1);
}
else if (inputString[i] == 'b') {
rotateDeg(B_DIR_PIN, B_STEP_PIN, -DEGREE_FACTOR, 1);
}
else if (inputString[i] == 'D') {
rotateDeg(D_DIR_PIN, D_STEP_PIN, DEGREE_FACTOR, 1);
}
else if (inputString[i] == 'd') {
rotateDeg(D_DIR_PIN, D_STEP_PIN, -DEGREE_FACTOR, 1);
}
else if (inputString[i] == 'L') {
rotateDeg(L_DIR_PIN, L_STEP_PIN, DEGREE_FACTOR, 1);
}
else if (inputString[i] == 'l') {
rotateDeg(L_DIR_PIN, L_STEP_PIN, -DEGREE_FACTOR, 1);
}
delay(300);
Serial.println(inputString[i]);
inputString = "";
stringComplete = false;
}
}
}
void goToGoal(int xpos, int ypos)
{
// Given a desired (x,y) coordinate, rotates and drives to the location
// if robot is not at (0,0) with roboangle ==0 , it adjusts accordingly
int xdelta = xpos - roboxpos;
int ydelta = ypos - roboypos;
float dist = sqrt((pow(xdelta,2)+pow(ydelta,2)))*12;
angle = atan2(ydelta,xdelta)*180/M_PI - roboangle;
rotateDeg(angle);
forward(dist);
roboxpos = xpos;
roboypos = ypos;
//updateLCD();
}
//----------------------------------------
void ofNode::rotate(float degrees, float vx, float vy, float vz) {
rotateDeg(degrees, vx, vy, vz);
}
//----------------------------------------
void ofNode::rotate(float degrees, const glm::vec3& v) {
rotateDeg(degrees, v);
}
//----------------------------------------
void ofNode::rollDeg(float degrees) {
rotateDeg(degrees, getZAxis());
}
//----------------------------------------
void ofNode::panDeg(float degrees) {
rotateDeg(degrees, getYAxis());
}
//----------------------------------------
void ofNode::tiltDeg(float degrees) {
rotateDeg(degrees, getXAxis());
}
//proportional control
char wallFollow(char side, float min, float max, float angle)
{
getIR();
if(ft<=2.5)
{
return 'e';
}
if(side=='r')
{
// Follow right wall
if(rt < min)
{
// move away
rotateDeg(2*angle);
forward(3*move);
return 'r';
}
else if (rt > 2*max)
{
// End of wall probably
// End routine
return 'e';
}
else if (rt > max){
// move towards
rotateDeg(-angle);
forward(move);
return 'r';
}
else
{
//move forwards
forward(move);
return 'r';
}
}else if(side=='l')
{
// Follow left wall
if(lt < min){
// too close
rotateDeg(-.75*angle);
forward(move);
return 'l';
}
else if (lt > 2*max){
// end routine
return 'e';
}
else if (lt > max){
// too far
rotateDeg(angle);
forward(3*move);
return 'l';
}
else
{
forward(move);
return 'l';
}
}
else
{
// Error
SPKR_beep(440);
SPKR_beep(440);
return 'x';
}
}
char shyGuy(short angle, float front, float right, float left, float back)
{
getIR(); // Reads sensors
if (frontRight && !frontLeft)
{
// Turn left
rotateDeg(45);
return 'f';
}
else if (frontLeft && !frontRight)
{
// Turn right
rotateDeg(-45);
return 'f';
}
else if (ft <= front || (frontRight && frontLeft)) // Checks left buffer
{
float leftfront;
float rightfront;
rotateDeg(30);
getIR();
leftfront = ft;
rotateDeg(-60);
getIR();
rightfront = ft;
rotateDeg(30);
if(leftfront >= rightfront)
{
rotateDeg(90);
forward(move);
return 'f';
}
else
{
rotateDeg(-90);
forward(move);
return 'f';
}
}
else if (rt <= right)
{
rotateDeg(30);
forward(move);
return 'r';
}
else if (lt <= left)
{
rotateDeg(-30);
forward(move);
return 'l';
}else if (bk <= back)
{
forward(move);
return 'b';
}else
{
// Calculate very small random angle increment
if(angle==1)
{
int angle = 21-randRange(1,40);
rotateDeg(angle); // Rotates
}
forward(move); // Moves forward
return 'w';
}
}
char avoidObstacle(float front, float right, float left, float back){
getIR(); // Reads sensors
if (frontRight && !frontLeft)
{
// Turn left
rotateDeg(45);
return 'f';
}
else if (frontLeft && !frontRight)
{
// Turn right
rotateDeg(-45);
return 'f';
}
else if (ft <= front || (frontRight && frontLeft)) // Checks left buffer
{
float leftfront;
float rightfront;
rotateDeg(30);
getIR();
leftfront = ft;
rotateDeg(-60);
getIR();
rightfront = ft;
rotateDeg(30);
if(leftfront >= rightfront)
{
rotateDeg(90);
forward(move);
return 'f';
}
else
{
rotateDeg(-90);
forward(move);
return 'f';
}
}
else if (rt <= right)
{
rotateDeg(30);
forward(move);
return 'r';
}
else if (lt <= left)
{
rotateDeg(-30);
forward(move);
return 'l';
}else if (bk <= back)
{
forward(move);
return 'b';
}else
{
// rotate toward goal
int xdelta = xpos - roboxpos;
int ydelta = ypos - roboypos;
angle = atan2(ydelta,xdelta)*180/M_PI - roboangle;
rotateDeg(angle);
forward(.5); // Moves forward
return 'w';
}
}
