movement_ctrl.c
Kod: Markera allt
#include "movement_ctrl.h"
#include <eyebot.h>
#include <math.h>
/***** motor control functions *****/
#define TRUE 1
#define FALSE 0
#define turnSPEED
#ifndef turnSPEED
#define turnSPEED 25
#endif
#define OK 0
#define ERROR -1
#ifndef PI
#define PI 3.14159265
#endif
/***** global variabels *****/
VWHandle vw;
SpeedType s;
MotorHandle leftMotor;
MotorHandle rightMotor;
PositionType* pos;
COMPASSInit comp;
/***** Init motor & return status *****/
int initMotor(void){
if(!MOTORInit(leftMotor)){
return FALSE;
}
if(!MOTORInit(rightMotor)){
return FALSE;
}
return TRUE;
}
/***** Release motor controll *****/
void releaseMotor(void){
MOTORRelease(rightMotor|leftMotor);
}
/***** Move forward *****/
void driveForward(int SPEED){
MOTORDrive(rightMotor,SPEED);
MOTORDrive(leftMotor,SPEED);
}
/***** Stop *****/
void driveStop(void){
MOTORDrive(rightMotor,0);
MOTORDrive(leftMotor,0);
}
/***** Reverse the robot *****/
void driveReverse(int SPEED){
MOTORDrive(rightMotor,-1*SPEED);
MOTORDrive(leftMotor,-1*SPEED);
}
/***** Turn right, change the speed in def *****/
void turnRight(void){
MOTORDrive(rightMotor,turnSPEED);
MOTORDrive(leftMotor,-1*turnSPEED);
}
/***** Turn left, change the speed in def *****/
void turnLeft(void){
MOTORDrive(rightMotor,-1*turnSPEED);
MOTORDrive(leftMotor,turnSPEED);
}
/***** Set the current position *****/
void setCord(int x, int y, int phi){
if(!VWSetPosition(vw, x, y, phi)){
LCDPutString("Cordinates FIX");
}
else{
LCDPutString("Cord ERROR");
}
}
/***** Drive to point (X,Y) *****/
void drive2p(int x, int y, int phi){
VWGetPosition(vw, &pos);
int X = pos.x;
int Y = pos.y;
//phi = atan((x-X)/(y-Y)); // in rad
/*if(X < x){
}*/
delta = ((x-X)^2+(y-Y)^2)^(1/2);
LCDPutString("Turning...");
VWDriveTurn(vw, phi, 1.2);
while (!VWDriveDone(vwHandle));
OSSleep(30);
LCDPutString("Driving to..");
LCDSetPos(x,y);
if(delta>0.3){
s.v=2.0;
}
else
{
s.v=0.5;
}
VWDriveStraight(vw, delta, s.v);
while (!VWDriveDone(vwHandle));
OSSleep(30);
setCord(x,y,phi);
}
/***** Start the digital compass and set the startpoints *****/
void startNavig(int x, int y, int phi){
/* COMPASSInit(VW_DRIVE);
COMPASSStart(1);
COMPASSCalibrate(0);*/
setCord(x,y,phi);
LCDSetPos(x,y);
}
/***** Init the advanced system with PI-controll *****/
/***** & a digital compass system with a cordinate system *****/
void initSYS(int x, int y, int phi){
vw = VWInit(VW_DRIVE,1); /* init v-omega interface */
if(vw == 0)
{
LCDPutString("VWInit ERROR!\n");
OSWait(200);
}
s.v = 0.0; s.w = 0.0;
startNavig(x, y, phi)
VWGetPosition(vw,&pos);
VWStartControl(vw, 7.0, 0.3, 7.0, 0.1);
VWSetSpeed(vw,s.v,s.w);
}
/**
* initialize the movement control structure.
* @param mv a pointer to the camera control structure to initalize
*/
void initMovementModule(move_t *move) {
move->m.init = initMotor;
move->m.release = releaseMotor;
move->m.stop = driveStop;
move->m.left = turnLeft;
move->m.right = turnRight;
move->Forward = driveForward;
move->Reverse = driveReverse;
move->setCord = setCord;
move->Drive2p = drive2p;
}
Kod: Markera allt
#ifndef MOVEMENT_CTRL_H
#define MOVEMENT_CTRL_H
#include "module.h"
typedef struct {
/***** Handles init & release *****/
/***** stop, turning left & right *****/
module_t m;
/***** Move the robot forward, set speed 0-30 *****/
void (*driveForward)(int SPEED);
/***** Move the robot backward, set speed 0-30 ****/
void (*driveReverse)(int SPEED);
/***** Set the current position *****/
void (*setCord)(int x, int y)
/***** Drive to point (X,Y) *****/
void (*drive2p)(int x, int y)
} move_t;
/**
* initialize the movement control structure.
* @param mv a pointer to the camera control structure to initalize
*/
void initMovementModule(move_t *mv);
#endif
fixade den ...men får fortfarande några fel 
