fatboyslim / Mbed 2 deprecated buttontest

Dependencies:   FRDM-TFC mbed

Committer:
bbbobbbieo
Date:
Mon Mar 02 19:57:48 2015 +0000
Revision:
22:f097de115024
Parent:
21:df5a530208eb
this shit sucks now

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bbbobbbieo 3:c7caa058fc50 1 //#include "mbed.h"
bbbobbbieo 1:21d40d90b2f0 2 #include "TFC.h"
bbbobbbieo 0:d57117b2188d 3
bbbobbbieo 0:d57117b2188d 4 DigitalOut myled(LED1);
bbbobbbieo 0:d57117b2188d 5
bbbobbbieo 14:f43b386b8b5d 6 int main()
bbbobbbieo 1:21d40d90b2f0 7 {
bbbobbbieo 7:455e7dd338ee 8 //run this before anything
bbbobbbieo 2:d8a51492b646 9 TFC_Init();
bbbobbbieo 14:f43b386b8b5d 10
bbbobbbieo 7:455e7dd338ee 11 //variables
bbbobbbieo 4:7584ff0426f1 12 float current_servo_position = 0;
bbbobbbieo 6:44d1079f076c 13 float current_left_motor_speed = 0;
bbbobbbieo 6:44d1079f076c 14 float current_right_motor_speed = 0;
mperella 17:c643b6b4a96f 15
mperella 17:c643b6b4a96f 16 float dt = 0.00001;
mperella 17:c643b6b4a96f 17 float integral = 0;
mperella 17:c643b6b4a96f 18 float derivative = 0;
mperella 17:c643b6b4a96f 19 float output = 0;
mperella 17:c643b6b4a96f 20
mperella 17:c643b6b4a96f 21 // gains on prop, int, der
mperella 17:c643b6b4a96f 22 // subject to change, need to fine tune
mperella 17:c643b6b4a96f 23 float kp = 1.8960;
mperella 17:c643b6b4a96f 24 float ki = 0.6170;
mperella 17:c643b6b4a96f 25 float kd = 1.5590;
mperella 17:c643b6b4a96f 26
bbbobbbieo 6:44d1079f076c 27 bool rear_motor_enable_flag = true;
bbbobbbieo 7:455e7dd338ee 28 bool linescan_ping_pong = false;
bbbobbbieo 9:2b028ee421ad 29 bool linescan_enable = true;
bbbobbbieo 14:f43b386b8b5d 30
bbbobbbieo 11:d65d6d7fc85e 31 int black_values_list[128];
bbbobbbieo 11:d65d6d7fc85e 32 int black_value_count = 0;
bbbobbbieo 11:d65d6d7fc85e 33 int black_center_value = 0;
bbbobbbieo 11:d65d6d7fc85e 34 int sum_black = 0;
bbbobbbieo 22:f097de115024 35 int black_center_value_past_1 =64;
bbbobbbieo 22:f097de115024 36 int black_center_value_current = 64;
bbbobbbieo 22:f097de115024 37
bbbobbbieo 14:f43b386b8b5d 38 int violence_level = 0;
bbbobbbieo 15:830209e846d5 39
bbbobbbieo 20:c728b8ffad97 40 int center_now = 64;
bbbobbbieo 20:c728b8ffad97 41 int center_past_1 = 64;
bbbobbbieo 20:c728b8ffad97 42 int center_past_2 = 64;
bbbobbbieo 20:c728b8ffad97 43 int center_past_3 = 64;
bbbobbbieo 20:c728b8ffad97 44 int center_past_4 = 64;
bbbobbbieo 20:c728b8ffad97 45 //int best_guess_center = 64;
bbbobbbieo 20:c728b8ffad97 46
bbbobbbieo 22:f097de115024 47
mperella 17:c643b6b4a96f 48 int set_point = 64;
bbbobbbieo 19:85eb7991e2ab 49 int previous_error = 0;
mperella 17:c643b6b4a96f 50 int error = 0;
mperella 17:c643b6b4a96f 51
mperella 17:c643b6b4a96f 52
bbbobbbieo 9:2b028ee421ad 53 //uint16_t MyImage0Buffer[2][128];
bbbobbbieo 9:2b028ee421ad 54 //uint16_t MyImage1Buffer[2][128];
bbbobbbieo 14:f43b386b8b5d 55
bbbobbbieo 7:455e7dd338ee 56 // major loop
bbbobbbieo 14:f43b386b8b5d 57 while(1) {
bbbobbbieo 14:f43b386b8b5d 58
bbbobbbieo 15:830209e846d5 59 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 60 if (TFC_ReadPushButton(0) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 61 current_servo_position = current_servo_position-.005;
bbbobbbieo 14:f43b386b8b5d 62 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 63 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 64 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 65 }// end check button0
bbbobbbieo 14:f43b386b8b5d 66
bbbobbbieo 15:830209e846d5 67 else {}
bbbobbbieo 14:f43b386b8b5d 68
bbbobbbieo 15:830209e846d5 69 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 70 if (TFC_ReadPushButton(1) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 71 current_servo_position = current_servo_position+.005;
bbbobbbieo 14:f43b386b8b5d 72 if(current_servo_position >= 0.4)
bbbobbbieo 14:f43b386b8b5d 73 current_servo_position = 0.4;
bbbobbbieo 14:f43b386b8b5d 74 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 75 }// end check button1
bbbobbbieo 14:f43b386b8b5d 76
bbbobbbieo 15:830209e846d5 77 else {}
bbbobbbieo 14:f43b386b8b5d 78
bbbobbbieo 15:830209e846d5 79 // initial motor stuff
bbbobbbieo 14:f43b386b8b5d 80 if(rear_motor_enable_flag) {
bbbobbbieo 14:f43b386b8b5d 81 TFC_HBRIDGE_ENABLE;
bbbobbbieo 14:f43b386b8b5d 82
bbbobbbieo 15:830209e846d5 83 //current_left_motor_speed = (TFC_ReadPot(0));
bbbobbbieo 15:830209e846d5 84 //current_right_motor_speed = (TFC_ReadPot(1));
bbbobbbieo 14:f43b386b8b5d 85
bbbobbbieo 15:830209e846d5 86 // checking behavior level
bbbobbbieo 14:f43b386b8b5d 87 violence_level = int(TFC_GetDIP_Switch());
bbbobbbieo 14:f43b386b8b5d 88
bbbobbbieo 15:830209e846d5 89 if (violence_level==2) {
bbbobbbieo 16:11ba5d6f42ba 90 current_left_motor_speed = -.48;
bbbobbbieo 16:11ba5d6f42ba 91 current_right_motor_speed = .48;
bbbobbbieo 15:830209e846d5 92 }
bbbobbbieo 15:830209e846d5 93 else if (violence_level==1) {
bbbobbbieo 16:11ba5d6f42ba 94 current_left_motor_speed = -.35;
bbbobbbieo 16:11ba5d6f42ba 95 current_right_motor_speed = .35;
bbbobbbieo 14:f43b386b8b5d 96 }
bbbobbbieo 21:df5a530208eb 97 else if (violence_level==3) {
bbbobbbieo 21:df5a530208eb 98 current_left_motor_speed = -.55;
bbbobbbieo 21:df5a530208eb 99 current_right_motor_speed = .55;
bbbobbbieo 21:df5a530208eb 100 }
bbbobbbieo 15:830209e846d5 101 else if (violence_level==0) {
bbbobbbieo 15:830209e846d5 102 current_left_motor_speed = 0;
bbbobbbieo 15:830209e846d5 103 current_right_motor_speed = 0;
bbbobbbieo 15:830209e846d5 104 }
bbbobbbieo 15:830209e846d5 105 else {
bbbobbbieo 15:830209e846d5 106 current_left_motor_speed = 0;
bbbobbbieo 15:830209e846d5 107 current_right_motor_speed = 0;
bbbobbbieo 15:830209e846d5 108 }
bbbobbbieo 15:830209e846d5 109
bbbobbbieo 14:f43b386b8b5d 110
bbbobbbieo 15:830209e846d5 111 // protection block
bbbobbbieo 21:df5a530208eb 112 if(current_left_motor_speed >= 0.7)
bbbobbbieo 21:df5a530208eb 113 current_left_motor_speed= 0.7;
bbbobbbieo 21:df5a530208eb 114 if(current_right_motor_speed >= 0.7)
bbbobbbieo 21:df5a530208eb 115 current_right_motor_speed= 0.7;
bbbobbbieo 21:df5a530208eb 116 if(current_left_motor_speed <= -0.7)
bbbobbbieo 21:df5a530208eb 117 current_left_motor_speed= -0.7;
bbbobbbieo 21:df5a530208eb 118 if(current_right_motor_speed <= -0.7)
bbbobbbieo 21:df5a530208eb 119 current_right_motor_speed= -0.7;
bbbobbbieo 14:f43b386b8b5d 120
bbbobbbieo 14:f43b386b8b5d 121 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 122 }// end motor enabled
bbbobbbieo 14:f43b386b8b5d 123 else {
bbbobbbieo 14:f43b386b8b5d 124 TFC_HBRIDGE_DISABLE;
bbbobbbieo 14:f43b386b8b5d 125 }// end motor disabled
bbbobbbieo 14:f43b386b8b5d 126
bbbobbbieo 15:830209e846d5 127 // camera stuff
bbbobbbieo 14:f43b386b8b5d 128 if (linescan_enable) {
bbbobbbieo 14:f43b386b8b5d 129 if (TFC_LineScanImageReady !=0) {
bbbobbbieo 14:f43b386b8b5d 130
bbbobbbieo 15:830209e846d5 131 if (linescan_ping_pong) {
bbbobbbieo 15:830209e846d5 132 //checking channel 0
bbbobbbieo 15:830209e846d5 133
bbbobbbieo 15:830209e846d5 134 //checking center pixel, displays aprox value on leds
bbbobbbieo 14:f43b386b8b5d 135 uint8_t shitnum = 1;
bbbobbbieo 14:f43b386b8b5d 136 if (*(TFC_LineScanImage0+64) > 800)
bbbobbbieo 14:f43b386b8b5d 137 shitnum = 15;
bbbobbbieo 14:f43b386b8b5d 138 else if((*(TFC_LineScanImage0+64) > 450))
bbbobbbieo 14:f43b386b8b5d 139 shitnum = 7;
bbbobbbieo 14:f43b386b8b5d 140 else if((*(TFC_LineScanImage0+64) > 400))
bbbobbbieo 14:f43b386b8b5d 141 shitnum = 3;
bbbobbbieo 14:f43b386b8b5d 142 else
bbbobbbieo 14:f43b386b8b5d 143 shitnum = 1;
bbbobbbieo 14:f43b386b8b5d 144 TFC_SetBatteryLED(shitnum);
bbbobbbieo 14:f43b386b8b5d 145
bbbobbbieo 15:830209e846d5 146
bbbobbbieo 15:830209e846d5 147 // checking for center line (single line)
bbbobbbieo 14:f43b386b8b5d 148 for (uint16_t i=0; i<128; i++) {
bbbobbbieo 22:f097de115024 149 if ((*(TFC_LineScanImage0+i) < 300)) {
bbbobbbieo 14:f43b386b8b5d 150 black_values_list[black_value_count] = i;
bbbobbbieo 14:f43b386b8b5d 151 black_value_count++;
bbbobbbieo 14:f43b386b8b5d 152 }
bbbobbbieo 14:f43b386b8b5d 153 }
bbbobbbieo 14:f43b386b8b5d 154
bbbobbbieo 14:f43b386b8b5d 155 for(int i=0; i<black_value_count; i++) {
bbbobbbieo 14:f43b386b8b5d 156 sum_black += black_values_list[i];
bbbobbbieo 14:f43b386b8b5d 157 }
bbbobbbieo 14:f43b386b8b5d 158
bbbobbbieo 20:c728b8ffad97 159 //update history
bbbobbbieo 20:c728b8ffad97 160 center_past_4= center_past_3;
bbbobbbieo 20:c728b8ffad97 161 center_past_3= center_past_2;
bbbobbbieo 20:c728b8ffad97 162 center_past_2= center_past_1;
bbbobbbieo 20:c728b8ffad97 163 center_past_1= center_now;
bbbobbbieo 20:c728b8ffad97 164
bbbobbbieo 15:830209e846d5 165 // value of center of black (single line)
bbbobbbieo 20:c728b8ffad97 166 //black_center_value = sum_black / black_value_count;
bbbobbbieo 20:c728b8ffad97 167 center_now = sum_black / black_value_count;
bbbobbbieo 20:c728b8ffad97 168
bbbobbbieo 20:c728b8ffad97 169 // best guess of center based on weighted average of history
bbbobbbieo 22:f097de115024 170 black_center_value_past_1 = black_center_value_current ;
bbbobbbieo 22:f097de115024 171 black_center_value_current = (5*center_now + 5*center_past_1 + 20*center_past_2 +30*center_past_3 +40*center_past_4)/100;
bbbobbbieo 22:f097de115024 172 black_center_value = (60*black_center_value_current + 40*black_center_value_past_1)/100;
bbbobbbieo 15:830209e846d5 173
mperella 17:c643b6b4a96f 174 /* ******* PID ALGORITHM *******
mperella 17:c643b6b4a96f 175
mperella 17:c643b6b4a96f 176 error = set_point - black_center_value;
mperella 17:c643b6b4a96f 177 integral = integral + error*dt;
mperella 17:c643b6b4a96f 178 derivative = (error - previous_error)/dt;
mperella 17:c643b6b4a96f 179 output = kp*error + ki*integral + kd*derivative;
mperella 17:c643b6b4a96f 180 previous error = error;
mperella 17:c643b6b4a96f 181
mperella 17:c643b6b4a96f 182 GOTTA DO SOME TESTS/SIMULATIONS TO CALIBRATE SERVO ADJUSTMENTS
mperella 17:c643b6b4a96f 183
mperella 17:c643b6b4a96f 184
mperella 17:c643b6b4a96f 185 *****************************
mperella 17:c643b6b4a96f 186 */
bbbobbbieo 14:f43b386b8b5d 187
bbbobbbieo 21:df5a530208eb 188 /*if (black_center_value == 64) {
bbbobbbieo 21:df5a530208eb 189
bbbobbbieo 21:df5a530208eb 190 TFC_SetServo(0, 0.0);
bbbobbbieo 21:df5a530208eb 191 if (violence_level !=0){
bbbobbbieo 21:df5a530208eb 192 current_left_motor_speed = current_left_motor_speed + violence_level*.045;// push more forwards
bbbobbbieo 21:df5a530208eb 193 current_right_motor_speed = current_right_motor_speed + violence_level*.045;// push more forwards
bbbobbbieo 21:df5a530208eb 194 }
bbbobbbieo 21:df5a530208eb 195 }*/
bbbobbbieo 21:df5a530208eb 196
bbbobbbieo 15:830209e846d5 197 // need to turn left
bbbobbbieo 15:830209e846d5 198 if (black_center_value < 64) {
bbbobbbieo 15:830209e846d5 199
bbbobbbieo 22:f097de115024 200 //current_servo_position= float(.01875*black_center_value-(1.2));
bbbobbbieo 22:f097de115024 201 //current_servo_position= float(.015625*black_center_value-(1.0));
bbbobbbieo 22:f097de115024 202 current_servo_position= float(.0234375*black_center_value-(1.5));
bbbobbbieo 14:f43b386b8b5d 203 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 204 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 205 TFC_SetServo(0, current_servo_position);
bbbobbbieo 15:830209e846d5 206
bbbobbbieo 15:830209e846d5 207
bbbobbbieo 16:11ba5d6f42ba 208 //current_left_motor_speed = current_left_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 209 //current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 210 if (violence_level !=0){
bbbobbbieo 21:df5a530208eb 211 current_left_motor_speed = current_left_motor_speed + float(float(64-black_center_value)*.035);// kinda reverse this...
bbbobbbieo 21:df5a530208eb 212 current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*.025);// push more forwards
bbbobbbieo 16:11ba5d6f42ba 213 }
bbbobbbieo 16:11ba5d6f42ba 214
bbbobbbieo 21:df5a530208eb 215 /*// protection block
bbbobbbieo 16:11ba5d6f42ba 216 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 217 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 218 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 219 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 220 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 221 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 222 if(current_right_motor_speed <= -0.5)
bbbobbbieo 21:df5a530208eb 223 current_right_motor_speed= -0.5;*/
bbbobbbieo 21:df5a530208eb 224
bbbobbbieo 21:df5a530208eb 225 // protection block
bbbobbbieo 21:df5a530208eb 226 if(current_left_motor_speed >= 0.7)
bbbobbbieo 21:df5a530208eb 227 current_left_motor_speed= 0.7;
bbbobbbieo 21:df5a530208eb 228 if(current_right_motor_speed >= 0.7)
bbbobbbieo 21:df5a530208eb 229 current_right_motor_speed= 0.7;
bbbobbbieo 21:df5a530208eb 230 if(current_left_motor_speed <= -0.7)
bbbobbbieo 21:df5a530208eb 231 current_left_motor_speed= -0.7;
bbbobbbieo 21:df5a530208eb 232 if(current_right_motor_speed <= -0.7)
bbbobbbieo 21:df5a530208eb 233 current_right_motor_speed= -0.7;
bbbobbbieo 16:11ba5d6f42ba 234
bbbobbbieo 14:f43b386b8b5d 235 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 236
bbbobbbieo 7:455e7dd338ee 237 }
bbbobbbieo 14:f43b386b8b5d 238
bbbobbbieo 15:830209e846d5 239 // need to turn right
bbbobbbieo 14:f43b386b8b5d 240 if (black_center_value > 64) {
bbbobbbieo 14:f43b386b8b5d 241
bbbobbbieo 22:f097de115024 242 //current_servo_position= float(.01875*black_center_value-(1.2));
bbbobbbieo 22:f097de115024 243 //current_servo_position= float(.015625*black_center_value-(1.0));
bbbobbbieo 22:f097de115024 244 current_servo_position= float(.0234375*black_center_value-(1.5));
bbbobbbieo 14:f43b386b8b5d 245 if( current_servo_position >= +0.4)
bbbobbbieo 14:f43b386b8b5d 246 current_servo_position = +0.4;
bbbobbbieo 14:f43b386b8b5d 247 TFC_SetServo(0, current_servo_position);
bbbobbbieo 15:830209e846d5 248
bbbobbbieo 16:11ba5d6f42ba 249 //current_left_motor_speed = current_left_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 250 //current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 251 if (violence_level !=0){
bbbobbbieo 21:df5a530208eb 252 current_left_motor_speed = current_left_motor_speed - float(float(black_center_value-64)*.025);// push more forwards
bbbobbbieo 21:df5a530208eb 253 current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-64)*.035);// kinda reverse this...
bbbobbbieo 16:11ba5d6f42ba 254 }
bbbobbbieo 15:830209e846d5 255
bbbobbbieo 21:df5a530208eb 256 /*// protection block
bbbobbbieo 16:11ba5d6f42ba 257 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 258 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 259 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 260 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 261 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 262 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 263 if(current_right_motor_speed <= -0.5)
bbbobbbieo 21:df5a530208eb 264 current_right_motor_speed= -0.5;*/
bbbobbbieo 21:df5a530208eb 265
bbbobbbieo 21:df5a530208eb 266 // protection block
bbbobbbieo 21:df5a530208eb 267 if(current_left_motor_speed >= 0.7)
bbbobbbieo 21:df5a530208eb 268 current_left_motor_speed= 0.7;
bbbobbbieo 21:df5a530208eb 269 if(current_right_motor_speed >= 0.7)
bbbobbbieo 21:df5a530208eb 270 current_right_motor_speed= 0.7;
bbbobbbieo 21:df5a530208eb 271 if(current_left_motor_speed <= -0.7)
bbbobbbieo 21:df5a530208eb 272 current_left_motor_speed= -0.7;
bbbobbbieo 21:df5a530208eb 273 if(current_right_motor_speed <= -0.7)
bbbobbbieo 21:df5a530208eb 274 current_right_motor_speed= -0.7;
bbbobbbieo 16:11ba5d6f42ba 275
bbbobbbieo 14:f43b386b8b5d 276 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 15:830209e846d5 277
bbbobbbieo 14:f43b386b8b5d 278 }
bbbobbbieo 14:f43b386b8b5d 279
bbbobbbieo 15:830209e846d5 280 // clearing values for next image processing round
bbbobbbieo 14:f43b386b8b5d 281 black_value_count = 0;
bbbobbbieo 14:f43b386b8b5d 282 black_center_value = 0;
bbbobbbieo 14:f43b386b8b5d 283 sum_black = 0;
bbbobbbieo 14:f43b386b8b5d 284
bbbobbbieo 15:830209e846d5 285 // end image processing
bbbobbbieo 14:f43b386b8b5d 286
bbbobbbieo 14:f43b386b8b5d 287 linescan_ping_pong = false;
bbbobbbieo 14:f43b386b8b5d 288 } // end checking channel 0
bbbobbbieo 15:830209e846d5 289
bbbobbbieo 15:830209e846d5 290 else { //checking channel 1
bbbobbbieo 14:f43b386b8b5d 291 linescan_ping_pong = true;
bbbobbbieo 14:f43b386b8b5d 292 }
bbbobbbieo 14:f43b386b8b5d 293
bbbobbbieo 7:455e7dd338ee 294 TFC_LineScanImageReady ==0; // since we used it, we reset the flag
bbbobbbieo 14:f43b386b8b5d 295 }// end imageready
bbbobbbieo 14:f43b386b8b5d 296 }// end linescan stuff
bbbobbbieo 0:d57117b2188d 297 }
bbbobbbieo 0:d57117b2188d 298 }
bbbobbbieo 8:946806df7347 299
bbbobbbieo 8:946806df7347 300
bbbobbbieo 15:830209e846d5 301 // shit code down here