easel/move.cpp at master · TNCTRobocon/easel · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
#include "config.hpp"
//#include "move.hpp"
#include <stdio.h>

#define sign_rf -1 //lb
#define sign_lf 1//rf
#define sign_rb -1//lf
#define sign_lb -1 //rb

void Front(float value){
  if(spflag==1){
		printf("flont %d\n",(int)value*100);
	}else{
		printf("flont %.2lf\n",value);
	}
  motordrive(tirerf,sign_rf*value);
  motordrive(tirerb,sign_rb*value);
  motordrive(tirelf,sign_lf*value);
  motordrive(tirelb,sign_lb*value);
}

void Back(float value){
  if(spflag==1){
		printf("back %d\n",(int)value*100);
	}else{
		printf("back %.2lf\n",value);
	}
  motordrive(tirerf,-sign_rf*value);
  motordrive(tirerb,-sign_rb*value);
  motordrive(tirelf,-sign_lf*value);
  motordrive(tirelb,-sign_lb*value);
}

void Right(float value){
  if(spflag==1){
		printf("right %d\n",(int)value*100);
	}else{
		printf("right %.2lf\n",value);
	}
  motordrive(tirerf,-sign_rf*value);
  motordrive(tirerb,sign_rb*value);
  motordrive(tirelf,sign_lf*value);
  motordrive(tirelb,-sign_lb*value);
}

void Left(float value){
  if(spflag==1){
		printf("left %d\n",(int)value*100);
	}else{
		printf("left %.2lf\n",value);
	}
  motordrive(tirerf,sign_rf*value);
  motordrive(tirerb,-sign_rb*value);
  motordrive(tirelf,-sign_lf*value);
  motordrive(tirelb,sign_lb*value);
}

void TurnRight(float value){
  if(spflag==1){
		printf("turn-right %d\n",(int)value*100);
	}else{
		printf("turn-right %.2lf\n",value);
	}
  motordrive(tirerf,-sign_rf*value);
  motordrive(tirerb,-sign_rb*value);
  motordrive(tirelf,sign_lf*value);
  motordrive(tirelb,sign_lb*value);
}

void TurnLeft(float value){
  if(spflag==1){
		printf("turn-left %d\n",(int)value*100);
	}else{
		printf("turn-left %.2lf\n",value);
	}
  motordrive(tirerf,sign_rf*value);
  motordrive(tirerb,sign_rb*value);//
  motordrive(tirelf,-sign_lf*value);
  motordrive(tirelb,-sign_lb*value);
}

void Stop(float value){
  value = 0;
  if(spflag==1){
		printf("stop %d\n",(int)value*100);
	}else{
		printf("stop %.2lf\n",value);
	}
  motordrive(tirerf,value);
  motordrive(tirerb,value);
  motordrive(tirelf,value);
  motordrive(tirelb,value);
}