TurtleBot / Mbed 2 deprecated TurtleBot_V6

Hormone V.1

Dependencies:   Servo mbed-rtos mbed

main.cpp@1:13164a15fbf6, 2018-04-19 (annotated)

Committer:
Khanchana
Date:
Thu Apr 19 01:54:55 2018 +0000
Revision:
1:13164a15fbf6
Parent:
0:43d21d5145d3 
alpha 0.8; beta 0.5

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Khanchana 0:43d21d5145d3 1 #include "mbed.h"
Khanchana 0:43d21d5145d3 2 #include "Servo.h"
Khanchana 0:43d21d5145d3 3 #include "rtos.h"
Khanchana 0:43d21d5145d3 4 #include "attitude.h"
Khanchana 1:13164a15fbf6 5 #include "math.h"
Khanchana 0:43d21d5145d3 6
Khanchana 0:43d21d5145d3 7 Serial pc(USBTX, USBRX);
Khanchana 0:43d21d5145d3 8
Khanchana 0:43d21d5145d3 9 Timer timer1;
Khanchana 0:43d21d5145d3 10 Timer timerwalk;
Khanchana 0:43d21d5145d3 11
Khanchana 0:43d21d5145d3 12 Thread thread1;
Khanchana 0:43d21d5145d3 13 Thread thread2;
Khanchana 0:43d21d5145d3 14
Khanchana 0:43d21d5145d3 15 Servo Servo1(D6);
Khanchana 0:43d21d5145d3 16 Servo Servo2(D8);
Khanchana 0:43d21d5145d3 17 Servo Servo3(D9);
Khanchana 0:43d21d5145d3 18 Servo Servo4(D10);
Khanchana 0:43d21d5145d3 19
Khanchana 0:43d21d5145d3 20 void IMU();
Khanchana 0:43d21d5145d3 21 void Cal_si();
Khanchana 0:43d21d5145d3 22 void Avg();
Khanchana 1:13164a15fbf6 23 void Cal_Cg_linear();
Khanchana 1:13164a15fbf6 24 void Cal_Cg_log();
Khanchana 0:43d21d5145d3 25 void move();
Khanchana 0:43d21d5145d3 26 void receive_hormone();
Khanchana 0:43d21d5145d3 27 void cal_step_down(); //calculate step
Khanchana 0:43d21d5145d3 28 void cal_step_up();
Khanchana 0:43d21d5145d3 29 void servo();
Khanchana 0:43d21d5145d3 30 void servo_Right();
Khanchana 0:43d21d5145d3 31
Khanchana 0:43d21d5145d3 32 int walking_time;
Khanchana 0:43d21d5145d3 33 int i = 0;
Khanchana 0:43d21d5145d3 34
Khanchana 0:43d21d5145d3 35 float avg = 0.00;
Khanchana 0:43d21d5145d3 36 float sum = 0.00;
Khanchana 0:43d21d5145d3 37 float roll_data[10];
Khanchana 0:43d21d5145d3 38 float Si = 0.00 ;
Khanchana 0:43d21d5145d3 39 float Cg = 0.00;
Khanchana 1:13164a15fbf6 40 float Cg_down = 0.00;
Khanchana 1:13164a15fbf6 41 float Cg_up = 0;
Khanchana 1:13164a15fbf6 42 float standardDeviation = 0;
Khanchana 1:13164a15fbf6 43 float sd = 0;
Khanchana 0:43d21d5145d3 44
Khanchana 0:43d21d5145d3 45 float pos_down_start = 1400.00;
Khanchana 0:43d21d5145d3 46 float pos_up_start = 1000.00;
Khanchana 0:43d21d5145d3 47 float down_degree = 90.00;
Khanchana 0:43d21d5145d3 48 float up_degree = 45.00;
Khanchana 0:43d21d5145d3 49 float stepmin = 1;
Khanchana 1:13164a15fbf6 50 float round = 5;
Khanchana 0:43d21d5145d3 51 float waittime = 0.001 ;
Khanchana 0:43d21d5145d3 52
Khanchana 0:43d21d5145d3 53 float pos_down_left = 1400.00;
Khanchana 0:43d21d5145d3 54 float pos_up_left = 1000.00;
Khanchana 0:43d21d5145d3 55 float pos_down_end_left;
Khanchana 0:43d21d5145d3 56 float pos_up_end_left;
Khanchana 0:43d21d5145d3 57 float state_count_left = 1;
Khanchana 0:43d21d5145d3 58 float round_count_left = 1;
Khanchana 0:43d21d5145d3 59 float step_down_left;
Khanchana 0:43d21d5145d3 60 float step_up_left;
Khanchana 0:43d21d5145d3 61
Khanchana 0:43d21d5145d3 62 float pos_down_right = 1400.00;
Khanchana 0:43d21d5145d3 63 float pos_up_right = 1000.00;
Khanchana 0:43d21d5145d3 64 float pos_down_end_right;
Khanchana 0:43d21d5145d3 65 float pos_up_end_right;
Khanchana 0:43d21d5145d3 66 float state_count_right = 1;
Khanchana 0:43d21d5145d3 67 float round_count_right = 1;
Khanchana 0:43d21d5145d3 68 float step_up_right;
Khanchana 0:43d21d5145d3 69 float step_down_right;
Khanchana 0:43d21d5145d3 70
Khanchana 0:43d21d5145d3 71
Khanchana 0:43d21d5145d3 72 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Khanchana 0:43d21d5145d3 73 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Khanchana 0:43d21d5145d3 74 //
Khanchana 0:43d21d5145d3 75 int main() { //
Khanchana 0:43d21d5145d3 76 pc.baud(1000000); //
Khanchana 0:43d21d5145d3 77 timer1.start(); // start timer counting //
Khanchana 0:43d21d5145d3 78 pc.printf("PRESS '1'\n"); //
Khanchana 0:43d21d5145d3 79 if (pc.getc() == '1'){ //
Khanchana 0:43d21d5145d3 80 thread2.start(servo); //
Khanchana 0:43d21d5145d3 81 } //
Khanchana 0:43d21d5145d3 82 } //
Khanchana 0:43d21d5145d3 83 //
Khanchana 0:43d21d5145d3 84 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Khanchana 0:43d21d5145d3 85 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Khanchana 0:43d21d5145d3 86
Khanchana 0:43d21d5145d3 87 ///////////////////////////Control SERVO////////////////////////////////////////
Khanchana 0:43d21d5145d3 88 ////////////////////////////////////////////////////////////////////////////////
Khanchana 0:43d21d5145d3 89 void servo() {
Khanchana 0:43d21d5145d3 90 pc.printf("start\n");
Khanchana 0:43d21d5145d3 91 attitude_setup();
Khanchana 0:43d21d5145d3 92 thread1.start(IMU);
Khanchana 0:43d21d5145d3 93 //wait(0.01);
Khanchana 0:43d21d5145d3 94 timerwalk.start(); // start timer counting
Khanchana 0:43d21d5145d3 95 move();
Khanchana 0:43d21d5145d3 96 //pc.printf("%f\n",roll_min);
Khanchana 0:43d21d5145d3 97 //pc.printf("%f\n",roll_max);
Khanchana 0:43d21d5145d3 98 }
Khanchana 0:43d21d5145d3 99
Khanchana 0:43d21d5145d3 100 void receive_hormone(){
Khanchana 1:13164a15fbf6 101 down_degree = 90.00*(1.00-(0.06*Cg_down));
Khanchana 1:13164a15fbf6 102 /*pc.printf("%f \t",Si);
Khanchana 1:13164a15fbf6 103 pc.printf("down\t");
Khanchana 1:13164a15fbf6 104 pc.printf("%f \t",down_degree);
Khanchana 1:13164a15fbf6 105 pc.printf("%f\t",Cg);*/
Khanchana 1:13164a15fbf6 106 up_degree = 45.00*(1.00+(0.7*Cg_up));
Khanchana 1:13164a15fbf6 107 /*pc.printf("up\t");
Khanchana 1:13164a15fbf6 108 pc.printf("%f \t",up_degree);
Khanchana 1:13164a15fbf6 109 pc.printf("%f\n",Cg);*/
Khanchana 1:13164a15fbf6 110 if(down_degree < 85){
Khanchana 1:13164a15fbf6 111 down_degree = 85;
Khanchana 0:43d21d5145d3 112 if(up_degree > 75){
Khanchana 0:43d21d5145d3 113 up_degree = 75;
Khanchana 0:43d21d5145d3 114 }
Khanchana 0:43d21d5145d3 115 }
Khanchana 0:43d21d5145d3 116 }
Khanchana 0:43d21d5145d3 117
Khanchana 0:43d21d5145d3 118 void cal_step_down(){
Khanchana 0:43d21d5145d3 119 //pc.printf("down");
Khanchana 0:43d21d5145d3 120 //pc.printf("%f \n",down_degree);
Khanchana 0:43d21d5145d3 121 pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree)));
Khanchana 0:43d21d5145d3 122 pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree)));
Khanchana 0:43d21d5145d3 123 if (pos_down_end_right > pos_down_end_left){
Khanchana 0:43d21d5145d3 124 step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start);
Khanchana 0:43d21d5145d3 125 step_down_left = stepmin;
Khanchana 0:43d21d5145d3 126 } else if (pos_down_end_right < pos_down_end_left){
Khanchana 0:43d21d5145d3 127 step_down_right = stepmin;
Khanchana 0:43d21d5145d3 128 step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
Khanchana 0:43d21d5145d3 129 } else{
Khanchana 0:43d21d5145d3 130 step_down_right = stepmin;
Khanchana 0:43d21d5145d3 131 step_down_left = stepmin;
Khanchana 0:43d21d5145d3 132 }
Khanchana 0:43d21d5145d3 133 /*pc.printf("pos_down_right");
Khanchana 0:43d21d5145d3 134 pc.printf("%f\n",pos_down_end_right);
Khanchana 0:43d21d5145d3 135 pc.printf("pos_down_left");
Khanchana 0:43d21d5145d3 136 pc.printf("%f\n",pos_down_end_left);
Khanchana 0:43d21d5145d3 137 pc.printf("step_down_right");
Khanchana 0:43d21d5145d3 138 pc.printf("%f\n",step_down_right);
Khanchana 0:43d21d5145d3 139 pc.printf("step_down_left");
Khanchana 0:43d21d5145d3 140 pc.printf("%f\n",step_down_left);*/
Khanchana 0:43d21d5145d3 141 }
Khanchana 0:43d21d5145d3 142
Khanchana 0:43d21d5145d3 143 void cal_step_up(){
Khanchana 0:43d21d5145d3 144 //pc.printf("up");
Khanchana 0:43d21d5145d3 145 //pc.printf("%f \n",up_degree);
Khanchana 0:43d21d5145d3 146 pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree));
Khanchana 0:43d21d5145d3 147 pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree));
Khanchana 0:43d21d5145d3 148 if (pos_up_end_right > pos_up_end_left){
Khanchana 0:43d21d5145d3 149 step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start);
Khanchana 0:43d21d5145d3 150 step_up_left = stepmin;
Khanchana 0:43d21d5145d3 151 } else if (pos_up_end_right < pos_up_end_left){
Khanchana 0:43d21d5145d3 152 step_up_right = stepmin;
Khanchana 0:43d21d5145d3 153 step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
Khanchana 0:43d21d5145d3 154 } else{
Khanchana 0:43d21d5145d3 155 step_up_right = stepmin;
Khanchana 0:43d21d5145d3 156 step_up_left = stepmin;
Khanchana 0:43d21d5145d3 157 }
Khanchana 0:43d21d5145d3 158 /*pc.printf("pos_up_right");
Khanchana 0:43d21d5145d3 159 pc.printf("%f\n",pos_up_end_right);
Khanchana 0:43d21d5145d3 160 pc.printf("pos_up_left");
Khanchana 0:43d21d5145d3 161 pc.printf("%f\n",pos_up_end_left);
Khanchana 0:43d21d5145d3 162 pc.printf("step_up_right");
Khanchana 0:43d21d5145d3 163 pc.printf("%f\n",step_up_right);;
Khanchana 0:43d21d5145d3 164 pc.printf("step_up_left");
Khanchana 0:43d21d5145d3 165 pc.printf("%f\n",step_up_left); */
Khanchana 0:43d21d5145d3 166 }
Khanchana 0:43d21d5145d3 167
Khanchana 0:43d21d5145d3 168 void move(){
Khanchana 1:13164a15fbf6 169 Servo1.Enable(1000,20000);
Khanchana 1:13164a15fbf6 170 Servo2.Enable(1000,20000);
Khanchana 1:13164a15fbf6 171 Servo3.Enable(1000,20000);
Khanchana 1:13164a15fbf6 172 Servo4.Enable(1000,20000);
Khanchana 1:13164a15fbf6 173
Khanchana 1:13164a15fbf6 174 /*pc.printf("Si\t");
Khanchana 1:13164a15fbf6 175 pc.printf("%f \t",Si);
Khanchana 1:13164a15fbf6 176 pc.printf("down\t");
Khanchana 1:13164a15fbf6 177 pc.printf("%f \t",down_degree);
Khanchana 1:13164a15fbf6 178 pc.printf("%f\t",Cg);
Khanchana 1:13164a15fbf6 179 pc.printf("up\t");
Khanchana 1:13164a15fbf6 180 pc.printf("%f \t",up_degree);
Khanchana 1:13164a15fbf6 181 pc.printf("%f\n",Cg);*/
Khanchana 0:43d21d5145d3 182 while(1) {
Khanchana 0:43d21d5145d3 183 cal_step_down();
Khanchana 0:43d21d5145d3 184 cal_step_up();
Khanchana 0:43d21d5145d3 185 servo_Right();
Khanchana 0:43d21d5145d3 186 if(state_count_left == 1) {
Khanchana 0:43d21d5145d3 187 Servo1.SetPosition(pos_down_left);
Khanchana 0:43d21d5145d3 188 wait(waittime);
Khanchana 0:43d21d5145d3 189 pos_down_left = pos_down_left + step_down_left;
Khanchana 0:43d21d5145d3 190 if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) {
Khanchana 0:43d21d5145d3 191 state_count_left = 2;
Khanchana 0:43d21d5145d3 192 }
Khanchana 0:43d21d5145d3 193 /*pc.printf("LAD");
Khanchana 0:43d21d5145d3 194 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 195 pc.printf("LAP");
Khanchana 0:43d21d5145d3 196 pc.printf("%f\n",pos_up_left);*/
Khanchana 0:43d21d5145d3 197 } else if(state_count_left == 2) {
Khanchana 0:43d21d5145d3 198 Servo2.SetPosition(pos_up_left);
Khanchana 0:43d21d5145d3 199 wait(waittime);
Khanchana 0:43d21d5145d3 200 pos_up_left = pos_up_left + step_up_left;
Khanchana 0:43d21d5145d3 201 if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
Khanchana 0:43d21d5145d3 202 state_count_left = 3;
Khanchana 0:43d21d5145d3 203 }
Khanchana 0:43d21d5145d3 204 /*pc.printf("LBD");
Khanchana 0:43d21d5145d3 205 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 206 pc.printf("LBP");
Khanchana 0:43d21d5145d3 207 pc.printf("%f\n",pos_up_left);*/
Khanchana 0:43d21d5145d3 208 } else if(state_count_left == 3) {
Khanchana 0:43d21d5145d3 209 Servo1.SetPosition(pos_down_left);
Khanchana 0:43d21d5145d3 210 wait(waittime);
Khanchana 0:43d21d5145d3 211 pos_down_left = pos_down_left - step_down_left;
Khanchana 0:43d21d5145d3 212 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
Khanchana 0:43d21d5145d3 213 state_count_left = 4;
Khanchana 0:43d21d5145d3 214 }
Khanchana 0:43d21d5145d3 215 /*pc.printf("LCD");
Khanchana 0:43d21d5145d3 216 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 217 pc.printf("LCP");
Khanchana 0:43d21d5145d3 218 pc.printf("%f\n",pos_up_left);*/
Khanchana 0:43d21d5145d3 219 } else if(state_count_left == 4) {
Khanchana 0:43d21d5145d3 220 Servo2.SetPosition(pos_up_left);
Khanchana 0:43d21d5145d3 221 wait(waittime);
Khanchana 0:43d21d5145d3 222 pos_up_left = pos_up_left - step_up_left;
Khanchana 0:43d21d5145d3 223 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) {
Khanchana 0:43d21d5145d3 224 state_count_left = 0;
Khanchana 0:43d21d5145d3 225 }
Khanchana 0:43d21d5145d3 226 /*pc.printf("LDD");
Khanchana 0:43d21d5145d3 227 pc.printf("%f\n",pos_down_left);
Khanchana 0:43d21d5145d3 228 pc.printf("LDP");
Khanchana 0:43d21d5145d3 229 pc.printf("%f\n",pos_up_left);*/
Khanchana 0:43d21d5145d3 230 } else if (state_count_left == 0 and round_count_left < round) {
Khanchana 0:43d21d5145d3 231 round_count_left = round_count_left+1;
Khanchana 0:43d21d5145d3 232 state_count_left = 1;
Khanchana 0:43d21d5145d3 233 pos_down_left = pos_down_start;
Khanchana 0:43d21d5145d3 234 pos_up_left = pos_up_start;
Khanchana 0:43d21d5145d3 235 } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round){
Khanchana 0:43d21d5145d3 236 thread1.terminate();
Khanchana 0:43d21d5145d3 237 pc.printf("Finish \n");
Khanchana 0:43d21d5145d3 238 walking_time = timerwalk.read_ms();
Khanchana 0:43d21d5145d3 239 pc.printf("Walking time = %d \n", walking_time);
Khanchana 0:43d21d5145d3 240 break;
Khanchana 0:43d21d5145d3 241 }
Khanchana 0:43d21d5145d3 242 }
Khanchana 0:43d21d5145d3 243 }
Khanchana 0:43d21d5145d3 244
Khanchana 0:43d21d5145d3 245 void servo_Right()
Khanchana 0:43d21d5145d3 246 {
Khanchana 0:43d21d5145d3 247 if(state_count_right == 1) {
Khanchana 0:43d21d5145d3 248 Servo3.SetPosition(pos_down_right);
Khanchana 0:43d21d5145d3 249 wait(waittime);
Khanchana 0:43d21d5145d3 250 pos_down_right = pos_down_right + step_down_right;
Khanchana 0:43d21d5145d3 251 if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) {
Khanchana 0:43d21d5145d3 252 state_count_right = 2;
Khanchana 0:43d21d5145d3 253 }
Khanchana 0:43d21d5145d3 254 /*pc.printf("RAD");
Khanchana 0:43d21d5145d3 255 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 256 pc.printf("RAP");
Khanchana 0:43d21d5145d3 257 pc.printf("%f\n",pos_up_right);*/
Khanchana 0:43d21d5145d3 258 } else if(state_count_right == 2) {
Khanchana 0:43d21d5145d3 259 Servo4.SetPosition(pos_up_right);
Khanchana 0:43d21d5145d3 260 wait(waittime);
Khanchana 0:43d21d5145d3 261 pos_up_right = pos_up_right + step_up_right;
Khanchana 0:43d21d5145d3 262 if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
Khanchana 0:43d21d5145d3 263 state_count_right = 3;
Khanchana 0:43d21d5145d3 264 }
Khanchana 0:43d21d5145d3 265 /*pc.printf("RBD");
Khanchana 0:43d21d5145d3 266 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 267 pc.printf("RBP");
Khanchana 0:43d21d5145d3 268 pc.printf("%f\n",pos_up_right);*/
Khanchana 0:43d21d5145d3 269 } else if(state_count_right == 3) {
Khanchana 0:43d21d5145d3 270 Servo3.SetPosition(pos_down_right);
Khanchana 0:43d21d5145d3 271 wait(waittime);
Khanchana 0:43d21d5145d3 272 pos_down_right = pos_down_right - step_down_right;
Khanchana 0:43d21d5145d3 273 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
Khanchana 0:43d21d5145d3 274 state_count_right = 4;
Khanchana 0:43d21d5145d3 275 }
Khanchana 0:43d21d5145d3 276 /*pc.printf("RCD");
Khanchana 0:43d21d5145d3 277 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 278 pc.printf("RCP");
Khanchana 0:43d21d5145d3 279 pc.printf("%f\n",pos_up_right);*/
Khanchana 0:43d21d5145d3 280 } else if(state_count_right == 4) {
Khanchana 0:43d21d5145d3 281 Servo4.SetPosition(pos_up_right);
Khanchana 0:43d21d5145d3 282 wait(waittime);
Khanchana 0:43d21d5145d3 283 pos_up_right = pos_up_right - step_up_right;
Khanchana 0:43d21d5145d3 284 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) {
Khanchana 0:43d21d5145d3 285 state_count_right = 0;
Khanchana 0:43d21d5145d3 286 }
Khanchana 0:43d21d5145d3 287 /*pc.printf("RDD");
Khanchana 0:43d21d5145d3 288 pc.printf("%f\n",pos_down_right);
Khanchana 0:43d21d5145d3 289 pc.printf("RDP");
Khanchana 0:43d21d5145d3 290 pc.printf("%f\n",pos_up_right);*/
Khanchana 0:43d21d5145d3 291 } else if (state_count_right == 0 and round_count_right < round) {
Khanchana 0:43d21d5145d3 292 round_count_right = round_count_right+1;
Khanchana 0:43d21d5145d3 293 state_count_right = 1;
Khanchana 0:43d21d5145d3 294 pos_down_right = pos_down_start;
Khanchana 0:43d21d5145d3 295 pos_up_right = pos_up_start;
Khanchana 0:43d21d5145d3 296 receive_hormone();
Khanchana 0:43d21d5145d3 297 }
Khanchana 0:43d21d5145d3 298 }
Khanchana 0:43d21d5145d3 299
Khanchana 0:43d21d5145d3 300 ///////////////////////////Control IMU////////////////////////////////////////
Khanchana 0:43d21d5145d3 301 ////////////////////////////////////////////////////////////////////////////////
Khanchana 0:43d21d5145d3 302 void IMU(){
Khanchana 1:13164a15fbf6 303 pc.printf("roll\t");
Khanchana 1:13164a15fbf6 304 pc.printf("Si\t");
Khanchana 1:13164a15fbf6 305 pc.printf("Cg\t");
Khanchana 1:13164a15fbf6 306 pc.printf("down\t");
Khanchana 1:13164a15fbf6 307 pc.printf("up\n");
Khanchana 1:13164a15fbf6 308
Khanchana 0:43d21d5145d3 309 while(1) {
Khanchana 0:43d21d5145d3 310 if (timer1.read_us() >=10000)// read time in ms
Khanchana 0:43d21d5145d3 311 {
Khanchana 0:43d21d5145d3 312 attitude_get();
Khanchana 0:43d21d5145d3 313
Khanchana 0:43d21d5145d3 314 //pc.printf(" %f \t", ax*10 );
Khanchana 0:43d21d5145d3 315 //pc.printf(" %f \t", ay*10 );
Khanchana 0:43d21d5145d3 316 //pc.printf(" %f \t", az*10-10); //cm/s*s
Khanchana 0:43d21d5145d3 317
Khanchana 1:13164a15fbf6 318 //pc.printf("%f\t %.0f \t %.0f \n\r", roll, pitch, yaw);
Khanchana 1:13164a15fbf6 319
Khanchana 1:13164a15fbf6 320 //pc.printf("up\t");
Khanchana 1:13164a15fbf6 321 //pc.printf("%f \t",up_degree);
Khanchana 1:13164a15fbf6 322 //pc.printf("%f\t",Cg);
Khanchana 1:13164a15fbf6 323 //pc.printf("down\t");
Khanchana 1:13164a15fbf6 324 //pc.printf("%f \t",down_degree);
Khanchana 1:13164a15fbf6 325 //pc.printf("%f\n",Cg);
Khanchana 0:43d21d5145d3 326
Khanchana 0:43d21d5145d3 327 timer1.reset(); // reset timer
Khanchana 0:43d21d5145d3 328 Cal_si();
Khanchana 0:43d21d5145d3 329 }
Khanchana 0:43d21d5145d3 330 }
Khanchana 0:43d21d5145d3 331 }
Khanchana 0:43d21d5145d3 332
Khanchana 0:43d21d5145d3 333 void Cal_si(){
Khanchana 0:43d21d5145d3 334 if(state_count_left == 4 or state_count_right == 4){
Khanchana 0:43d21d5145d3 335 roll_data[i] = roll;
Khanchana 1:13164a15fbf6 336 //pc.printf("%f\n",roll_data[i]);
Khanchana 0:43d21d5145d3 337 Avg();
Khanchana 0:43d21d5145d3 338 //pc.printf("Avg ");
Khanchana 0:43d21d5145d3 339 //pc.printf("%f\n",avg);
Khanchana 1:13164a15fbf6 340 Cal_Cg_linear();
Khanchana 1:13164a15fbf6 341 //pc.printf("roll\t");
Khanchana 1:13164a15fbf6 342 pc.printf("%f\t",roll_data[i]);
Khanchana 1:13164a15fbf6 343 //pc.printf("Si\t");
Khanchana 1:13164a15fbf6 344 pc.printf("%f\t",Si);
Khanchana 1:13164a15fbf6 345 //pc.printf("Cg\t");
Khanchana 1:13164a15fbf6 346 pc.printf("%f\t",Cg);
Khanchana 1:13164a15fbf6 347 //pc.printf("down\t");
Khanchana 1:13164a15fbf6 348 pc.printf("%f \t",down_degree);
Khanchana 1:13164a15fbf6 349 //pc.printf("up\t");
Khanchana 1:13164a15fbf6 350 pc.printf("%f \n",up_degree);
Khanchana 0:43d21d5145d3 351 if(i == 9){
Khanchana 0:43d21d5145d3 352 i = 0;
Khanchana 0:43d21d5145d3 353 }else{
Khanchana 0:43d21d5145d3 354 i = i+1;
Khanchana 0:43d21d5145d3 355 }
Khanchana 0:43d21d5145d3 356 }
Khanchana 0:43d21d5145d3 357 }
Khanchana 0:43d21d5145d3 358
Khanchana 0:43d21d5145d3 359 void Avg(){
Khanchana 0:43d21d5145d3 360 sum = sum + roll;
Khanchana 0:43d21d5145d3 361 avg = sum/10;
Khanchana 1:13164a15fbf6 362 standardDeviation = (roll - avg)*(roll-avg);
Khanchana 1:13164a15fbf6 363 sd = sqrt(standardDeviation/10);
Khanchana 0:43d21d5145d3 364 if(avg < 0){
Khanchana 0:43d21d5145d3 365 avg = avg*(-1);
Khanchana 0:43d21d5145d3 366 }
Khanchana 0:43d21d5145d3 367 }
Khanchana 0:43d21d5145d3 368
Khanchana 1:13164a15fbf6 369
Khanchana 1:13164a15fbf6 370
Khanchana 1:13164a15fbf6 371 void Cal_Cg_linear(){
Khanchana 1:13164a15fbf6 372 Si = sd/0.6;
Khanchana 1:13164a15fbf6 373 sum = sum - roll_data[i];
Khanchana 0:43d21d5145d3 374 if(Si > 0){
Khanchana 1:13164a15fbf6 375 Cg = (0.8*Si)+(0.5*Cg);
Khanchana 0:43d21d5145d3 376 if(Cg > 1){
Khanchana 0:43d21d5145d3 377 Cg = 1;
Khanchana 0:43d21d5145d3 378 }
Khanchana 0:43d21d5145d3 379 }else{
Khanchana 0:43d21d5145d3 380 Cg = 0.00;
Khanchana 0:43d21d5145d3 381 }
Khanchana 1:13164a15fbf6 382 Cg_down = 1/0.1*exp(2*(Cg-2.081))-0.15;
Khanchana 1:13164a15fbf6 383 Cg_up = (0.8*log(Cg+0.4))+0.734;
Khanchana 0:43d21d5145d3 384 }