Ghoda/Ghoda.ino at master · SushantSangle/Ghoda · 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
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#include "stdInc.h"
//36 switch 37,38,39 push
Servo up[4],down[4];      //SERVO
int ukkhai_m1 = 8;
int ukkhai_m2 = 10;
int ukkhai_pwm = 9;
//use time limit in while for input form mpu
//time interval
//switch(36) left 1 right 0
//after step(37)
//dune(38)
int S = 2;
int b11=0,b=0;
//-----------PROXY---------------
bool pr,pf,pR;
int pMode=0,pMode1=0;
int fp=A0;//A2
int rp=A2;
int RP=A1;

//-------------------------------

//----------FOR_MPU--------------

int yaw2=0,pitch2=0,roll2=0;
float yaw,pitch,roll;
float yp=0,pp=0,rpr=0;
char ypr1[6];
int baseY=0;
bool q=0;
int timen=0,timep=0;
int pDiff[4] = {0,36,36,0};
int ARENA;
//-------------------------------

/*-------------------PINS-------------------------*/

const uint16_t upS[4]   = {49,47,40,43};
//{38,22,26,40}  {49,42,47,40}
const uint16_t downS[4] = {48,46,41,42};
//{34,28,24,36}{48,43,46,41}
/*------------------------------------------------*/

/*-------------------OFFSET-----------------------*/
/*-----------------------------------------------*/
/*---------------------MULTIPLIERS----------------*/
float Hm[4];
float Lm[4];
const int upB[4]=  {1 ,-1 ,-1 ,1  };
const int downB[4]={1 ,-1 ,-1 ,1  };
int m;
bool I_increment = false;
/*------------------------------------------------*/

/*------------POSITION_MANIPULATION---------------*/
const int elb[2]={0,410};      //elbow co-ordinates
int els[4][2];
bool mode=0;
int LEG,LEGP;
double xpriv[4]= {0,0,0,0};
double xnow[4] = {0,0,0,0};
/*------------------------------------------------*/

/*---------------ANGLE_MANIPULATION---------------*/
int rot[4]={0,0,0,0},rot1=0,i=0;             //Rotation of axis
float slope=0;
int Angle[4][72][2];
/*------------------------------------------------*/
/*-------------------STEP-COUNT-------------------*/
int steps =0;
int Step=0;
int startC=0;
bool runMode=0,pstate=1 ;
bool gobi=0;
/*------------------------------------------------*/

void setup()
{
  pinmode();

//  staticG();
//  test();
	alternate();
	gerejeStop();
	initial();
	getangle(4);
	initial();

  while(digitalRead(RP))
  {
    ARENA = digitalRead(36);
    Serial.println(digitalRead(RP));
    if(digitalRead(37))  afterDune();
    if(digitalRead(38))  mountain();
    if(digitalRead(39))  test();
  }
  while(!digitalRead(RP)){
    Serial.println(digitalRead(RP));
  }
//  while(digitalRead(fp)){
//
//  }
//  while(digitalRead(rp));
  delay(200);
  turn(ARENA);
  getangle(4);
//  mountain();
  Serial.println("RUN START");
  q=true;
  baseY = yaw;
//  gobi = true;
}
bool DUNE= false;
void loop()
{
  stepCount();
//  MPU();
  RUN(0);
  proxy();
//  duneMode();
//  shift();

}

/*----------------------POINTS_FOR_ALTERNATE----------------------*/

double pt[72][2];
/*----------------------------------------------------------------*/