balancingBot/main.cpp at master · riderman10000/balancingBot · 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
94
95
96
97
98
99
100
101
102
103
104
105
106
#include<stddef.h>
#include<avr/io.h>
#include<util/delay.h>
#include<stdlib.h>
#include <inttypes.h>

#include<stdio.h>
#include<math.h>
#include<avr/interrupt.h>

//#include "i2c.cpp"
#include "mpu6050.cpp"
//#include "mpu_define.h"
#include "usart.cpp"
#include "motor.cpp"
#include "filter.cpp"

#define F_CPU 16000000L
#define F_SCL 400000L

#define PI 3.14159265

float angle;
volatile float prevAngle;
volatile int i;
Usart usart;
Mpu6050 mpu;
Filter f;
Motor m;


float checkAngle()
{
	mpu.burst_read();
	f = mpu.v;

	return f.getAngle();
}


int main(void)
{

	/*float rb=0.155,rw=0.035;	//radius of bot and wheel respectively
	float g = 9.8;//m/s^2
	float rps;
	//angle;
	*/
	//float x;
	TCCR2 |= (1 << WGM21) ;
	TCCR2 |= (1 << CS20) | (1<<CS21) | ( 1<<CS22);

	TCNT2 = 0;

	OCR2 = 75;//75;

	TIMSK |= (1 << OCIE2);

	sei();

	while(1)
	{
		mpu.display(usart);
		usart.send("\r\nmanupulated value\n\r");
		usart.send(m.mv);

		usart.send("\r\nangle value\n\r");
		usart.send(angle);

		//x= checkAngle();

		/*if((angle>=0)&&(x>=0))
		{
			usart.send("\r\nin backward\n\r");
			m.backward();
			//while(angle<=0);
		}
		if((angle<0)&&(x<0))
		{
			usart.send("\r\nin forward\n\r");
			m.forward();
			//while(angle>=0);
		}*/
	}
}

ISR(TIMER2_COMP_vect)
{
	mpu.burst_read();
	f = mpu.v;
	mpu.v.pitch = f.getAngle();
	angle = mpu.v.pitch;
	m.pid(angle);
	if(angle>0)
	{
		//usart.send("\r\nin backward\n\r");
		m.backward();
		//while(angle<=0);
	}
	if(angle<0)
	{
		//usart.send("\r\nin forward\n\r");
		m.forward();
		//while(angle>=0);
	}
}