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

newAlgorithm.cpp@42:c189b914931d, 2015-03-10 (annotated)

Committer:
mperella
Date:
Tue Mar 10 15:51:02 2015 +0000
Revision:
42:c189b914931d
Parent:
41:79ccb6f70ab6
Child:
43:9f541718be01
added a little stuff to new algorithm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mperella 40:cdb079fc9d0a 1 #include "TFC.h"
mperella 40:cdb079fc9d0a 2 #include <iostream>
mperella 40:cdb079fc9d0a 3 #include <stdio.h>
mperella 40:cdb079fc9d0a 4
mperella 40:cdb079fc9d0a 5 const float proportional = 0;
mperella 40:cdb079fc9d0a 6 const float last_proportional = 0;
mperella 40:cdb079fc9d0a 7 const float integral = 0;
mperella 40:cdb079fc9d0a 8 const float derivative = 0;
mperella 40:cdb079fc9d0a 9 const float output = 0;
mperella 40:cdb079fc9d0a 10
mperella 41:79ccb6f70ab6 11 // these are definitely not right
mperella 40:cdb079fc9d0a 12 const float kp = 0.05;
mperella 40:cdb079fc9d0a 13 const float ki = 0.02;
mperella 40:cdb079fc9d0a 14 const float kd = 0.02;
mperella 40:cdb079fc9d0a 15
mperella 40:cdb079fc9d0a 16 const int set_point = 63;
mperella 40:cdb079fc9d0a 17 const int position = 0;
mperella 40:cdb079fc9d0a 18 const int error = 0;
mperella 40:cdb079fc9d0a 19
mperella 40:cdb079fc9d0a 20 const float AGGRESSIVE = .60;
mperella 40:cdb079fc9d0a 21 const float MODERATE =.45;
mperella 40:cdb079fc9d0a 22 const float CONSERVATIVE =.30;
mperella 40:cdb079fc9d0a 23 const float STOP =0;
mperella 40:cdb079fc9d0a 24
mperella 42:c189b914931d 25 const bool linescan_enable = true;
mperella 42:c189b914931d 26 const bool image = true;
mperella 42:c189b914931d 27
mperella 40:cdb079fc9d0a 28 const int black_values_list[LINE_SCAN_LENGTH];
mperella 40:cdb079fc9d0a 29 const int black_value_count = 0;
mperella 40:cdb079fc9d0a 30 const int black_center_value = 0;
mperella 40:cdb079fc9d0a 31 const int sum_black = 0;
mperella 40:cdb079fc9d0a 32 const int violence_level = 0;
mperella 40:cdb079fc9d0a 33
mperella 40:cdb079fc9d0a 34 const float current_left_motor_speed = 0;
mperella 40:cdb079fc9d0a 35 const float current_right_motor_speed = 0;
mperella 40:cdb079fc9d0a 36
mperella 40:cdb079fc9d0a 37 const float current_right_max = 0;
mperella 40:cdb079fc9d0a 38 const float current_left_max = 0;
mperella 40:cdb079fc9d0a 39
mperella 40:cdb079fc9d0a 40
mperella 40:cdb079fc9d0a 41 int main()
mperella 40:cdb079fc9d0a 42 {
mperella 40:cdb079fc9d0a 43 TFC_Init();
mperella 40:cdb079fc9d0a 44 if(rear_motor_enable_flag)
mperella 40:cdb079fc9d0a 45 {
mperella 40:cdb079fc9d0a 46 TFC_HBRIDGE_ENABLE;
mperella 40:cdb079fc9d0a 47
mperella 40:cdb079fc9d0a 48 //current_left_motor_speed = (TFC_ReadPot(0));
mperella 40:cdb079fc9d0a 49 //current_right_motor_speed = (TFC_ReadPot(1));
mperella 40:cdb079fc9d0a 50
mperella 40:cdb079fc9d0a 51 // checking behavior level
mperella 40:cdb079fc9d0a 52 violence_level = int(TFC_GetDIP_Switch());
mperella 40:cdb079fc9d0a 53
mperella 40:cdb079fc9d0a 54 if (violence_level==3) {
mperella 40:cdb079fc9d0a 55 current_left_motor_speed = -(AGGRESSIVE);
mperella 40:cdb079fc9d0a 56 current_right_motor_speed = AGGRESSIVE;
mperella 40:cdb079fc9d0a 57 current_right_max = AGGRESSIVE;
mperella 40:cdb079fc9d0a 58 current_left_max = -AGGRESSIVE;
mperella 40:cdb079fc9d0a 59
mperella 40:cdb079fc9d0a 60 }
mperella 40:cdb079fc9d0a 61 else if (violence_level==2) {
mperella 40:cdb079fc9d0a 62 current_left_motor_speed = -(MODERATE);
mperella 40:cdb079fc9d0a 63 current_right_motor_speed = (MODERATE);
mperella 40:cdb079fc9d0a 64 current_right_max = MODERATE;
mperella 40:cdb079fc9d0a 65 current_left_max = -MODERATE;
mperella 40:cdb079fc9d0a 66 Kp = Kp * 0.75;
mperella 40:cdb079fc9d0a 67 Ki = Ki * 0.75;
mperella 40:cdb079fc9d0a 68 Kd = Kd * 0.75;
mperella 40:cdb079fc9d0a 69 }
mperella 40:cdb079fc9d0a 70 else if (violence_level==1) {
mperella 40:cdb079fc9d0a 71 current_left_motor_speed = -(CONSERVATIVE);
mperella 40:cdb079fc9d0a 72 current_right_motor_speed = CONSERVATIVE;
mperella 40:cdb079fc9d0a 73 current_right_max = CONSERVATIVE;
mperella 40:cdb079fc9d0a 74 current_left_max = CONSERVATIVE;
mperella 40:cdb079fc9d0a 75 Kp = Kp * 0.5;
mperella 40:cdb079fc9d0a 76 Ki = Ki * 0.5;
mperella 40:cdb079fc9d0a 77 Kd = Kd * 0.5;
mperella 40:cdb079fc9d0a 78 }
mperella 40:cdb079fc9d0a 79 else if (violence_level==0) {
mperella 40:cdb079fc9d0a 80 current_left_motor_speed = STOP;
mperella 40:cdb079fc9d0a 81 current_right_motor_speed = STOP;
mperella 40:cdb079fc9d0a 82 }
mperella 40:cdb079fc9d0a 83 else {
mperella 40:cdb079fc9d0a 84 current_left_motor_speed = STOP;
mperella 40:cdb079fc9d0a 85 current_right_motor_speed = STOP;
mperella 40:cdb079fc9d0a 86 }
mperella 40:cdb079fc9d0a 87
mperella 40:cdb079fc9d0a 88
mperella 40:cdb079fc9d0a 89 // protection block
mperella 40:cdb079fc9d0a 90 /*
mperella 40:cdb079fc9d0a 91 if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER)
mperella 40:cdb079fc9d0a 92 current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
mperella 40:cdb079fc9d0a 93 if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
mperella 40:cdb079fc9d0a 94 current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
mperella 40:cdb079fc9d0a 95 if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
mperella 40:cdb079fc9d0a 96 current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
mperella 40:cdb079fc9d0a 97 if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER)
mperella 40:cdb079fc9d0a 98 current_right_motor_speed = PROTECTION_THRESHOLD_LOWER;
mperella 40:cdb079fc9d0a 99 */
mperella 40:cdb079fc9d0a 100
mperella 40:cdb079fc9d0a 101 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 40:cdb079fc9d0a 102 }// end motor enabled
mperella 40:cdb079fc9d0a 103 else {
mperella 40:cdb079fc9d0a 104 TFC_HBRIDGE_DISABLE;
mperella 40:cdb079fc9d0a 105 }// end motor disabled
mperella 40:cdb079fc9d0a 106
mperella 40:cdb079fc9d0a 107 }
mperella 40:cdb079fc9d0a 108
mperella 40:cdb079fc9d0a 109 void loop()
mperella 40:cdb079fc9d0a 110 {
mperella 40:cdb079fc9d0a 111 if (linescan_enable)
mperella 40:cdb079fc9d0a 112 {
mperella 40:cdb079fc9d0a 113 if (TFC_LineScanImageReady !=0)
mperella 40:cdb079fc9d0a 114 {
mperella 42:c189b914931d 115 // first image
mperella 42:c189b914931d 116 position = getSensorValues(image); // get position
mperella 42:c189b914931d 117 error = pid_calc(position); // send position to PID algorithm to get error
mperella 42:c189b914931d 118 calcMotors(error); // send error to calculate both motors
mperella 42:c189b914931d 119
mperella 42:c189b914931d 120 // second image
mperella 42:c189b914931d 121 position = getSensorValues(image);
mperella 42:c189b914931d 122 error = pid_calc(position);
mperella 42:c189b914931d 123 calcMotors(error);
mperella 40:cdb079fc9d0a 124 }
mperella 40:cdb079fc9d0a 125 }
mperella 40:cdb079fc9d0a 126 else
mperella 40:cdb079fc9d0a 127 {
mperella 40:cdb079fc9d0a 128 TFC_SetMotorPWM(STOP, STOP);
mperella 40:cdb079fc9d0a 129 }
mperella 40:cdb079fc9d0a 130 }
mperella 40:cdb079fc9d0a 131
mperella 42:c189b914931d 132 int getSensorValues(bool image)
mperella 40:cdb079fc9d0a 133 {
mperella 42:c189b914931d 134 if(image)
mperella 40:cdb079fc9d0a 135 {
mperella 42:c189b914931d 136 for (uint16_t i=0; i<128; i++)
mperella 40:cdb079fc9d0a 137 {
mperella 42:c189b914931d 138 if ((*(TFC_LineScanImage1+i) < 250))
mperella 42:c189b914931d 139 {
mperella 42:c189b914931d 140 black_values_list[black_value_count] = i;
mperella 42:c189b914931d 141 black_value_count++;
mperella 42:c189b914931d 142 }
mperella 40:cdb079fc9d0a 143 }
mperella 42:c189b914931d 144
mperella 42:c189b914931d 145 image = false;
mperella 42:c189b914931d 146 }
mperella 42:c189b914931d 147 else
mperella 42:c189b914931d 148 {
mperella 42:c189b914931d 149 for (uint16_t i=0; i<128; i++)
mperella 42:c189b914931d 150 {
mperella 42:c189b914931d 151 if ((*(TFC_LineScanImage0+i) < 250))
mperella 42:c189b914931d 152 {
mperella 42:c189b914931d 153 black_values_list[black_value_count] = i;
mperella 42:c189b914931d 154 black_value_count++;
mperella 42:c189b914931d 155 }
mperella 42:c189b914931d 156 }
mperella 42:c189b914931d 157 image = true;
mperella 40:cdb079fc9d0a 158 }
mperella 40:cdb079fc9d0a 159
mperella 40:cdb079fc9d0a 160 for(int i=0; i<black_value_count; i++)
mperella 40:cdb079fc9d0a 161 {
mperella 40:cdb079fc9d0a 162 sum_black += black_values_list[i];
mperella 40:cdb079fc9d0a 163 }
mperella 40:cdb079fc9d0a 164
mperella 42:c189b914931d 165 black_center_value = int(sum_black / black_value_count);
mperella 42:c189b914931d 166 return black_center_value;
mperella 42:c189b914931d 167
mperella 40:cdb079fc9d0a 168 }
mperella 40:cdb079fc9d0a 169
mperella 40:cdb079fc9d0a 170 int pid_calc(int pos)
mperella 40:cdb079fc9d0a 171 {
mperella 40:cdb079fc9d0a 172 position = pos;
mperella 40:cdb079fc9d0a 173 proportional = position - set_point;
mperella 40:cdb079fc9d0a 174 integral = integral + proportional;
mperella 40:cdb079fc9d0a 175 derivative= proportional - last_proportional;
mperella 40:cdb079fc9d0a 176 last_proportional = proportional;
mperella 40:cdb079fc9d0a 177 error = proportional*Kp + integral*Ki + derivative*Kd;
mperella 41:79ccb6f70ab6 178 return error;
mperella 40:cdb079fc9d0a 179 }
mperella 40:cdb079fc9d0a 180
mperella 40:cdb079fc9d0a 181 void calcMotors(float error)
mperella 40:cdb079fc9d0a 182 {
mperella 40:cdb079fc9d0a 183 TFC_SetServo(0, error);
mperella 40:cdb079fc9d0a 184
mperella 40:cdb079fc9d0a 185 if(error < 0)
mperella 40:cdb079fc9d0a 186 {
mperella 40:cdb079fc9d0a 187 current_left_motor_speed = current_left_motor_speed + error; // reverse this
mperella 40:cdb079fc9d0a 188
mperella 40:cdb079fc9d0a 189 // make sure not going too fast
mperella 40:cdb079fc9d0a 190 if(current_right_motor_speed > current_right_max)
mperella 40:cdb079fc9d0a 191 {
mperella 40:cdb079fc9d0a 192 current_right_motor_speed = current_right_max;
mperella 40:cdb079fc9d0a 193 }
mperella 40:cdb079fc9d0a 194 else
mperella 40:cdb079fc9d0a 195 {
mperella 40:cdb079fc9d0a 196 current_right_motor_speed = current_right_motor_speed + error;
mperella 40:cdb079fc9d0a 197 }
mperella 40:cdb079fc9d0a 198 }
mperella 40:cdb079fc9d0a 199 else
mperella 40:cdb079fc9d0a 200 {
mperella 40:cdb079fc9d0a 201 current_right_motor_speed = current_right_motor_speed - error;
mperella 40:cdb079fc9d0a 202
mperella 40:cdb079fc9d0a 203 // make sure not going too fast
mperella 40:cdb079fc9d0a 204 if(current_left_motor_speed < current_left_max)
mperella 40:cdb079fc9d0a 205 {
mperella 40:cdb079fc9d0a 206 current_left_motor_speed = current_left_max;
mperella 40:cdb079fc9d0a 207 }
mperella 40:cdb079fc9d0a 208 else
mperella 40:cdb079fc9d0a 209 {
mperella 40:cdb079fc9d0a 210 current_left_motor_speed = current_left_motor_speed - error;
mperella 40:cdb079fc9d0a 211 }
mperella 40:cdb079fc9d0a 212 }
mperella 40:cdb079fc9d0a 213
mperella 40:cdb079fc9d0a 214 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 40:cdb079fc9d0a 215 }