TurtleBot / Mbed 2 deprecated TurtleBot_v01

turtlebot v 01

Dependencies:   Servo mbed-rtos mbed PM hormone

Fork of TurtleBot_v01 by worasuchad haomachai

main.cpp@0:43d21d5145d3, 2018-04-10 (annotated)

Committer:
Khanchana
Date:
Tue Apr 10 13:42:04 2018 +0000
Revision:
0:43d21d5145d3
Child:
1:13164a15fbf6
little turtlebot with Hormone

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