fatboyslim / Mbed 2 deprecated doublelinedoublescan

shit code for winning the race

Dependencies:   FRDM-TFC mbed

main.cpp@0:113ea09a1ea9, 2015-03-24 (annotated)

Committer:
bbbobbbieo
Date:
Tue Mar 24 15:15:15 2015 +0000
Revision:
0:113ea09a1ea9
made new program thats shitty

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bbbobbbieo 0:113ea09a1ea9 1 //#include "mbed.h"
bbbobbbieo 0:113ea09a1ea9 2 #include "TFC.h"
bbbobbbieo 0:113ea09a1ea9 3 //#include <iostream>
bbbobbbieo 0:113ea09a1ea9 4 //#include <stdio.h>
bbbobbbieo 0:113ea09a1ea9 5 //#include "serialib.h"
bbbobbbieo 0:113ea09a1ea9 6
bbbobbbieo 0:113ea09a1ea9 7 const float AGGRESSIVE = .55;
bbbobbbieo 0:113ea09a1ea9 8 const float MODERATE =.48;
bbbobbbieo 0:113ea09a1ea9 9 const float CONSERVATIVE =.35;
bbbobbbieo 0:113ea09a1ea9 10 const float STOP =0;
bbbobbbieo 0:113ea09a1ea9 11 const float PROTECTION_THRESHOLD_UPPER =.7;
bbbobbbieo 0:113ea09a1ea9 12 const float PROTECTION_THRESHOLD_LOWER =-.7;
bbbobbbieo 0:113ea09a1ea9 13 const float TURN_FORWARD_ACCEL =0.045;
bbbobbbieo 0:113ea09a1ea9 14 const float TURN_BACKWARD_ACCEL =0.025;
bbbobbbieo 0:113ea09a1ea9 15 const float SERVO_CAN_MOVE_IN_ONE_FRAME =0.1;
bbbobbbieo 0:113ea09a1ea9 16 const float SERVO_MAX =.5;
bbbobbbieo 0:113ea09a1ea9 17
bbbobbbieo 0:113ea09a1ea9 18 const int BLACK_THRESHOLD =63;
bbbobbbieo 0:113ea09a1ea9 19 const int LINE_SCAN_LENGTH =128;
bbbobbbieo 0:113ea09a1ea9 20
bbbobbbieo 0:113ea09a1ea9 21
bbbobbbieo 0:113ea09a1ea9 22 DigitalOut myled(LED1);
bbbobbbieo 0:113ea09a1ea9 23
bbbobbbieo 0:113ea09a1ea9 24 int main()
bbbobbbieo 0:113ea09a1ea9 25 {
bbbobbbieo 0:113ea09a1ea9 26 //run this before anything
bbbobbbieo 0:113ea09a1ea9 27 TFC_Init();
bbbobbbieo 0:113ea09a1ea9 28
bbbobbbieo 0:113ea09a1ea9 29 //variables
bbbobbbieo 0:113ea09a1ea9 30 float current_servo_position = 0;
bbbobbbieo 0:113ea09a1ea9 31 float previous_servo_position = 0;
bbbobbbieo 0:113ea09a1ea9 32 float current_left_motor_speed = 0;
bbbobbbieo 0:113ea09a1ea9 33 float current_right_motor_speed = 0;
bbbobbbieo 0:113ea09a1ea9 34
bbbobbbieo 0:113ea09a1ea9 35 float proportional = 0;
bbbobbbieo 0:113ea09a1ea9 36 float last_proportional = 0;
bbbobbbieo 0:113ea09a1ea9 37 float integral = 0;
bbbobbbieo 0:113ea09a1ea9 38 float derivative = 0;
bbbobbbieo 0:113ea09a1ea9 39 float output = 0;
bbbobbbieo 0:113ea09a1ea9 40
bbbobbbieo 0:113ea09a1ea9 41 // gains on prop, int, der
bbbobbbieo 0:113ea09a1ea9 42 // subject to change, need to fine tune
bbbobbbieo 0:113ea09a1ea9 43 float kp = 1.8960;
bbbobbbieo 0:113ea09a1ea9 44 float ki = 0.6170;
bbbobbbieo 0:113ea09a1ea9 45 float kd = 1.5590;
bbbobbbieo 0:113ea09a1ea9 46
bbbobbbieo 0:113ea09a1ea9 47 bool rear_motor_enable_flag = true;
bbbobbbieo 0:113ea09a1ea9 48 bool linescan_ping_pong = false;
bbbobbbieo 0:113ea09a1ea9 49 bool linescan_enable = true;
bbbobbbieo 0:113ea09a1ea9 50
bbbobbbieo 0:113ea09a1ea9 51 int black_values_list[LINE_SCAN_LENGTH];
bbbobbbieo 0:113ea09a1ea9 52 int black_value_count = 0;
bbbobbbieo 0:113ea09a1ea9 53 int black_center_value = 0;
bbbobbbieo 0:113ea09a1ea9 54 int sum_black = 0;
bbbobbbieo 0:113ea09a1ea9 55 int violence_level = 0;
bbbobbbieo 0:113ea09a1ea9 56
bbbobbbieo 0:113ea09a1ea9 57 int accelList[3];
bbbobbbieo 0:113ea09a1ea9 58 int lastAccessed = 0;
bbbobbbieo 0:113ea09a1ea9 59
bbbobbbieo 0:113ea09a1ea9 60 int centers_List[50];
bbbobbbieo 0:113ea09a1ea9 61
bbbobbbieo 0:113ea09a1ea9 62 int center_now = 63;
bbbobbbieo 0:113ea09a1ea9 63 int center_past_1 = 63;
bbbobbbieo 0:113ea09a1ea9 64 int center_past_2 = 63;
bbbobbbieo 0:113ea09a1ea9 65 int center_past_3 = 63;
bbbobbbieo 0:113ea09a1ea9 66 int center_past_4 = 63;
bbbobbbieo 0:113ea09a1ea9 67 //int best_guess_center = 64;
bbbobbbieo 0:113ea09a1ea9 68
bbbobbbieo 0:113ea09a1ea9 69 int position = 0;
bbbobbbieo 0:113ea09a1ea9 70 int set_point = 63;
bbbobbbieo 0:113ea09a1ea9 71 int previous_error = 0;
bbbobbbieo 0:113ea09a1ea9 72 int error = 0;
bbbobbbieo 0:113ea09a1ea9 73
bbbobbbieo 0:113ea09a1ea9 74 for(int i = 0; i < 50; i++)
bbbobbbieo 0:113ea09a1ea9 75 centers_List[i] = 63;
bbbobbbieo 0:113ea09a1ea9 76
bbbobbbieo 0:113ea09a1ea9 77 int line_left_at;
bbbobbbieo 0:113ea09a1ea9 78 int line_right_at;
bbbobbbieo 0:113ea09a1ea9 79 bool line_left = false;
bbbobbbieo 0:113ea09a1ea9 80 bool line_right = false;
bbbobbbieo 0:113ea09a1ea9 81 int left_counter =0;
bbbobbbieo 0:113ea09a1ea9 82 int right_counter =0;
bbbobbbieo 0:113ea09a1ea9 83
bbbobbbieo 0:113ea09a1ea9 84
bbbobbbieo 0:113ea09a1ea9 85 // major loop
bbbobbbieo 0:113ea09a1ea9 86 while(1) {
bbbobbbieo 0:113ea09a1ea9 87
bbbobbbieo 0:113ea09a1ea9 88 // initial motor stuff
bbbobbbieo 0:113ea09a1ea9 89 if(rear_motor_enable_flag) {
bbbobbbieo 0:113ea09a1ea9 90 TFC_HBRIDGE_ENABLE;
bbbobbbieo 0:113ea09a1ea9 91
bbbobbbieo 0:113ea09a1ea9 92 // checking behavior level
bbbobbbieo 0:113ea09a1ea9 93 violence_level = int(TFC_GetDIP_Switch());
bbbobbbieo 0:113ea09a1ea9 94
bbbobbbieo 0:113ea09a1ea9 95 if (violence_level==3) {
bbbobbbieo 0:113ea09a1ea9 96 current_left_motor_speed = -(AGGRESSIVE);
bbbobbbieo 0:113ea09a1ea9 97 current_right_motor_speed = AGGRESSIVE;
bbbobbbieo 0:113ea09a1ea9 98 }
bbbobbbieo 0:113ea09a1ea9 99 else if (violence_level==2) {
bbbobbbieo 0:113ea09a1ea9 100 current_left_motor_speed = -(MODERATE);
bbbobbbieo 0:113ea09a1ea9 101 current_right_motor_speed = (MODERATE);
bbbobbbieo 0:113ea09a1ea9 102 }
bbbobbbieo 0:113ea09a1ea9 103 else if (violence_level==1) {
bbbobbbieo 0:113ea09a1ea9 104 current_left_motor_speed = -(CONSERVATIVE);
bbbobbbieo 0:113ea09a1ea9 105 current_right_motor_speed = CONSERVATIVE;
bbbobbbieo 0:113ea09a1ea9 106 }
bbbobbbieo 0:113ea09a1ea9 107 else if (violence_level==0) {
bbbobbbieo 0:113ea09a1ea9 108 current_left_motor_speed = STOP;
bbbobbbieo 0:113ea09a1ea9 109 current_right_motor_speed = STOP;
bbbobbbieo 0:113ea09a1ea9 110 }
bbbobbbieo 0:113ea09a1ea9 111 else {
bbbobbbieo 0:113ea09a1ea9 112 current_left_motor_speed = STOP;
bbbobbbieo 0:113ea09a1ea9 113 current_right_motor_speed = STOP;
bbbobbbieo 0:113ea09a1ea9 114 }
bbbobbbieo 0:113ea09a1ea9 115
bbbobbbieo 0:113ea09a1ea9 116
bbbobbbieo 0:113ea09a1ea9 117 // protection block
bbbobbbieo 0:113ea09a1ea9 118 if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER)
bbbobbbieo 0:113ea09a1ea9 119 current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
bbbobbbieo 0:113ea09a1ea9 120 if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
bbbobbbieo 0:113ea09a1ea9 121 current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
bbbobbbieo 0:113ea09a1ea9 122 if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
bbbobbbieo 0:113ea09a1ea9 123 current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
bbbobbbieo 0:113ea09a1ea9 124 if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER)
bbbobbbieo 0:113ea09a1ea9 125 current_right_motor_speed = PROTECTION_THRESHOLD_LOWER;
bbbobbbieo 0:113ea09a1ea9 126
bbbobbbieo 0:113ea09a1ea9 127 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 0:113ea09a1ea9 128 }// end motor enabled
bbbobbbieo 0:113ea09a1ea9 129 else {
bbbobbbieo 0:113ea09a1ea9 130 TFC_HBRIDGE_DISABLE;
bbbobbbieo 0:113ea09a1ea9 131 }// end motor disabled
bbbobbbieo 0:113ea09a1ea9 132
bbbobbbieo 0:113ea09a1ea9 133 // camera stuff
bbbobbbieo 0:113ea09a1ea9 134 if (linescan_enable) {
bbbobbbieo 0:113ea09a1ea9 135 if (TFC_LineScanImageReady !=0) {
bbbobbbieo 0:113ea09a1ea9 136
bbbobbbieo 0:113ea09a1ea9 137 if (linescan_ping_pong) {
bbbobbbieo 0:113ea09a1ea9 138 //checking channel 0
bbbobbbieo 0:113ea09a1ea9 139
bbbobbbieo 0:113ea09a1ea9 140 for (uint16_t i=100; i>0; i--) {
bbbobbbieo 0:113ea09a1ea9 141 line_left = (*(TFC_LineScanImage0+i) < 250) and (*(TFC_LineScanImage0+i-1) < 250) and (*(TFC_LineScanImage0+i-2) < 250);
bbbobbbieo 0:113ea09a1ea9 142 if (line_left) {
bbbobbbieo 0:113ea09a1ea9 143 line_left_at = i;
bbbobbbieo 0:113ea09a1ea9 144 i=-1;
bbbobbbieo 0:113ea09a1ea9 145 TFC_SetBatteryLED(1);
bbbobbbieo 0:113ea09a1ea9 146 }
bbbobbbieo 0:113ea09a1ea9 147 }
bbbobbbieo 0:113ea09a1ea9 148
bbbobbbieo 0:113ea09a1ea9 149 for (uint16_t i=40; i<128; i++) {
bbbobbbieo 0:113ea09a1ea9 150 line_right = (*(TFC_LineScanImage1+i) < 250) and (*(TFC_LineScanImage1+i+1) < 250) and (*(TFC_LineScanImage1+i+2) < 250);
bbbobbbieo 0:113ea09a1ea9 151 if (line_right) {
bbbobbbieo 0:113ea09a1ea9 152 line_right_at = i;
bbbobbbieo 0:113ea09a1ea9 153 i=129;
bbbobbbieo 0:113ea09a1ea9 154 TFC_SetBatteryLED(4);
bbbobbbieo 0:113ea09a1ea9 155 }
bbbobbbieo 0:113ea09a1ea9 156 }
bbbobbbieo 0:113ea09a1ea9 157
bbbobbbieo 0:113ea09a1ea9 158
bbbobbbieo 0:113ea09a1ea9 159 // turn left
bbbobbbieo 0:113ea09a1ea9 160 if (line_right) {
bbbobbbieo 0:113ea09a1ea9 161
bbbobbbieo 0:113ea09a1ea9 162 }
bbbobbbieo 0:113ea09a1ea9 163
bbbobbbieo 0:113ea09a1ea9 164 // turn right
bbbobbbieo 0:113ea09a1ea9 165 if (line_left) {
bbbobbbieo 0:113ea09a1ea9 166
bbbobbbieo 0:113ea09a1ea9 167 }
bbbobbbieo 0:113ea09a1ea9 168
bbbobbbieo 0:113ea09a1ea9 169 // clearing values for next image processing round
bbbobbbieo 0:113ea09a1ea9 170 line_left_at=63;
bbbobbbieo 0:113ea09a1ea9 171 line_right_at=63;
bbbobbbieo 0:113ea09a1ea9 172 line_left = false;
bbbobbbieo 0:113ea09a1ea9 173 line_right = false;
bbbobbbieo 0:113ea09a1ea9 174
bbbobbbieo 0:113ea09a1ea9 175
bbbobbbieo 0:113ea09a1ea9 176 // end image processing
bbbobbbieo 0:113ea09a1ea9 177 linescan_ping_pong = false;
bbbobbbieo 0:113ea09a1ea9 178 } // end checking channel 0
bbbobbbieo 0:113ea09a1ea9 179
bbbobbbieo 0:113ea09a1ea9 180 else { //checking channel 1
bbbobbbieo 0:113ea09a1ea9 181 linescan_ping_pong = true;
bbbobbbieo 0:113ea09a1ea9 182 }
bbbobbbieo 0:113ea09a1ea9 183
bbbobbbieo 0:113ea09a1ea9 184 TFC_LineScanImageReady = 0; // since we used it, we reset the flag
bbbobbbieo 0:113ea09a1ea9 185 }// end imageready
bbbobbbieo 0:113ea09a1ea9 186 }// end linescan stuff
bbbobbbieo 0:113ea09a1ea9 187 }//end while true
bbbobbbieo 0:113ea09a1ea9 188 }//end main
bbbobbbieo 0:113ea09a1ea9 189
bbbobbbieo 0:113ea09a1ea9 190
bbbobbbieo 0:113ea09a1ea9 191 /* shitcode
bbbobbbieo 0:113ea09a1ea9 192 left at bool, int
bbbobbbieo 0:113ea09a1ea9 193 right at bool, int
bbbobbbieo 0:113ea09a1ea9 194
bbbobbbieo 0:113ea09a1ea9 195 left turn counter
bbbobbbieo 0:113ea09a1ea9 196 right turn counter
bbbobbbieo 0:113ea09a1ea9 197
bbbobbbieo 0:113ea09a1ea9 198 if bool
bbbobbbieo 0:113ea09a1ea9 199 inc counter by proportional to int (where line is)
bbbobbbieo 0:113ea09a1ea9 200 turn away proportional to counter
bbbobbbieo 0:113ea09a1ea9 201 multiply counter by .75 (turn less)
bbbobbbieo 0:113ea09a1ea9 202
bbbobbbieo 0:113ea09a1ea9 203 */
bbbobbbieo 0:113ea09a1ea9 204