fatboyslim / Mbed 2 deprecated buttontest

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FRDM-TFC mbed

main.cpp@25:1d0f586aaf0c, 2015-03-03 (annotated)

Committer:
bbergandy
Date:
Tue Mar 03 18:16:34 2015 +0000
Revision:
25:1d0f586aaf0c
Parent:
24:504a58daa06e
Child:
27:3dd1460365d3
Child:
31:0c2b49175036
Cleaned up code

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