Janus Bo Andersen / Mbed 2 deprecated T2PRO1_master

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:   mbed m3pi

main.cpp@17:c2e21d347ca5, 2018-12-02 (annotated)

Committer:
janusboandersen
Date:
Sun Dec 02 15:10:57 2018 +0000
Revision:
17:c2e21d347ca5
Parent:
16:48b81a9246e6
Child:
18:8b3efa4d4a36
Add decision tree to main and list of functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:78f9794620a3 1 #include "mbed.h"
chris 0:78f9794620a3 2 #include "m3pi.h"
chris 0:78f9794620a3 3
chris 0:78f9794620a3 4 m3pi m3pi;
chris 0:78f9794620a3 5
chris 0:78f9794620a3 6 // Minimum and maximum motor speeds
Blunsdon 13:6a8689f601cd 7 #define MAX 0.3
chris 0:78f9794620a3 8 #define MIN 0
chris 0:78f9794620a3 9
chris 0:78f9794620a3 10 // PID terms
chris 0:78f9794620a3 11 #define P_TERM 1
chris 0:78f9794620a3 12 #define I_TERM 0
Blunsdon 1:47ea51d9e25d 13 #define D_TERM 8
Blunsdon 1:47ea51d9e25d 14
Blunsdon 12:dfc717775297 15 void sensor_all(int arr[]) //lav en funktion? ved ikke hvad jeg skal her
Blunsdon 1:47ea51d9e25d 16 {
Blunsdon 12:dfc717775297 17 m3pi.putc(0x87); // send noget sensor data
Blunsdon 1:47ea51d9e25d 18
janusboandersen 4:5939f9ad88ca 19 for(int n=0; n < 5; n++) // forloop så sensor for data
Blunsdon 1:47ea51d9e25d 20 {
Blunsdon 1:47ea51d9e25d 21 char lowbyte = m3pi.getc();
Blunsdon 1:47ea51d9e25d 22 char hibyte = m3pi.getc();
Blunsdon 12:dfc717775297 23 arr[n] = ((lowbyte + (hibyte << 8))); // sensor value
Blunsdon 1:47ea51d9e25d 24 }
Blunsdon 9:fa745d3afcac 25 m3pi.cls();
Blunsdon 9:fa745d3afcac 26 m3pi.locate(0,1);
Blunsdon 12:dfc717775297 27 m3pi.printf("%d", arr[0]);
Blunsdon 12:dfc717775297 28 m3pi.locate(0,0);
Blunsdon 12:dfc717775297 29 m3pi.printf("%d", arr[4]);
Blunsdon 11:394ab193971f 30 }
Blunsdon 13:6a8689f601cd 31
Blunsdon 13:6a8689f601cd 32 int maze (void)
Blunsdon 13:6a8689f601cd 33 {
Blunsdon 13:6a8689f601cd 34
Blunsdon 13:6a8689f601cd 35 return 1;
Blunsdon 14:408d9d087d0f 36 }
Blunsdon 14:408d9d087d0f 37
Blunsdon 14:408d9d087d0f 38 void turn_left (void)
Blunsdon 14:408d9d087d0f 39 {
Blunsdon 14:408d9d087d0f 40 m3pi.left(0.3);
Blunsdon 14:408d9d087d0f 41 wait (0.22);
Blunsdon 14:408d9d087d0f 42 }
Blunsdon 14:408d9d087d0f 43
Blunsdon 13:6a8689f601cd 44 void turn_right (void)
Blunsdon 13:6a8689f601cd 45 {
Blunsdon 13:6a8689f601cd 46 m3pi.right (0.3);
Blunsdon 14:408d9d087d0f 47 wait (0.22);
Blunsdon 13:6a8689f601cd 48 }
chris 0:78f9794620a3 49
chris 0:78f9794620a3 50 int main() {
janusboandersen 2:d22dcc2bfcc1 51
chris 0:78f9794620a3 52 m3pi.locate(0,1);
chris 0:78f9794620a3 53 m3pi.printf("Line PID");
chris 0:78f9794620a3 54
Blunsdon 1:47ea51d9e25d 55 wait(1.0);
chris 0:78f9794620a3 56
chris 0:78f9794620a3 57 m3pi.sensor_auto_calibrate();
chris 0:78f9794620a3 58
chris 0:78f9794620a3 59 float right;
chris 0:78f9794620a3 60 float left;
chris 0:78f9794620a3 61 float current_pos_of_line = 0.0;
chris 0:78f9794620a3 62 float previous_pos_of_line = 0.0;
chris 0:78f9794620a3 63 float derivative,proportional,integral = 0;
chris 0:78f9794620a3 64 float power;
chris 0:78f9794620a3 65 float speed = MAX;
Blunsdon 12:dfc717775297 66 int sensor_val[5];
chris 0:78f9794620a3 67
janusboandersen 17:c2e21d347ca5 68
janusboandersen 17:c2e21d347ca5 69 /* if restartedFromOff: Load values from file, recalibrate and start.
janusboandersen 17:c2e21d347ca5 70 */
janusboandersen 17:c2e21d347ca5 71
chris 0:78f9794620a3 72 while (1) {
janusboandersen 17:c2e21d347ca5 73
janusboandersen 17:c2e21d347ca5 74 /*
janusboandersen 17:c2e21d347ca5 75 switch
janusboandersen 17:c2e21d347ca5 76 if nothing else: line following
janusboandersen 17:c2e21d347ca5 77 if low_batt: slow down and continue
janusboandersen 17:c2e21d347ca5 78 if atPitIntersection: increment internal lap counter
janusboandersen 17:c2e21d347ca5 79 if low_batt && atPitIntersection: Stop && execute pit navigation program
janusboandersen 17:c2e21d347ca5 80 if all_sensors == 1000 (lifted from track):
janusboandersen 17:c2e21d347ca5 81 show Wh and pitstops on display
janusboandersen 17:c2e21d347ca5 82 stop motors, reset PID errors, wait 20 secs, recalibrate sensors
janusboandersen 17:c2e21d347ca5 83
janusboandersen 17:c2e21d347ca5 84 if all_sensors < 90 (driven off track):
janusboandersen 17:c2e21d347ca5 85 stop and ask for help? Flash LEDs? turn back and continue? Reset PID errors
janusboandersen 17:c2e21d347ca5 86
janusboandersen 17:c2e21d347ca5 87 Periodically (not necessarily every lap - maybe every 2-5th lap):
janusboandersen 17:c2e21d347ca5 88 Save to file -> possibly only after pit
janusboandersen 17:c2e21d347ca5 89 Check battery voltage -> every couple of minutes
janusboandersen 17:c2e21d347ca5 90 Check light sensors for drift -> recalibrate (optional)
janusboandersen 17:c2e21d347ca5 91
janusboandersen 17:c2e21d347ca5 92 */
janusboandersen 17:c2e21d347ca5 93
janusboandersen 17:c2e21d347ca5 94
janusboandersen 17:c2e21d347ca5 95
janusboandersen 17:c2e21d347ca5 96 /* Separate functions
janusboandersen 17:c2e21d347ca5 97 pit_navigation (entering pit from T-intersection all the way to the charger)
janusboandersen 17:c2e21d347ca5 98 pit_execution (only when connected to charger - voltage > V_CHARGER_MIN
janusboandersen 17:c2e21d347ca5 99 add 1 to number of pitstops
janusboandersen 17:c2e21d347ca5 100 charge and wait for signal from the external charging circuit
janusboandersen 17:c2e21d347ca5 101 during charging: integrate v(t) dt, add to cumulative sum
janusboandersen 17:c2e21d347ca5 102 when fully charged (or signal goes high):
janusboandersen 17:c2e21d347ca5 103 Calculate delta_Wh ( integrate{ v(t) dt} * u * 1/3600 )
janusboandersen 17:c2e21d347ca5 104 Calculate total_Wh
janusboandersen 17:c2e21d347ca5 105 save progress data to file
janusboandersen 17:c2e21d347ca5 106 leave the charger -> pass control to pit_exit
janusboandersen 17:c2e21d347ca5 107
janusboandersen 17:c2e21d347ca5 108 pit_exit
janusboandersen 17:c2e21d347ca5 109 reverse out of the pit back to T-intersection
janusboandersen 17:c2e21d347ca5 110 continue line following away from T-intersection, recal sensors after a few seconds
janusboandersen 17:c2e21d347ca5 111
janusboandersen 17:c2e21d347ca5 112
janusboandersen 17:c2e21d347ca5 113 AfterPitAdmin: Calibrate sensors, save state to file,
janusboandersen 17:c2e21d347ca5 114
janusboandersen 17:c2e21d347ca5 115 show_results_on_display:
janusboandersen 17:c2e21d347ca5 116
janusboandersen 17:c2e21d347ca5 117 check_sensors_for_errors:
janusboandersen 17:c2e21d347ca5 118
janusboandersen 17:c2e21d347ca5 119 check_battery_voltage -> return true false
janusboandersen 17:c2e21d347ca5 120 compare m3pi.battery() < V_CRITICAL
janusboandersen 17:c2e21d347ca5 121
janusboandersen 17:c2e21d347ca5 122
janusboandersen 17:c2e21d347ca5 123
janusboandersen 17:c2e21d347ca5 124 HUSK FOR POKKER KOLLES KODESTANDARD!
janusboandersen 17:c2e21d347ca5 125
janusboandersen 17:c2e21d347ca5 126 */
janusboandersen 17:c2e21d347ca5 127
janusboandersen 17:c2e21d347ca5 128
Blunsdon 11:394ab193971f 129 // Get the position of the line.
chris 0:78f9794620a3 130 current_pos_of_line = m3pi.line_position();
chris 0:78f9794620a3 131 proportional = current_pos_of_line;
chris 0:78f9794620a3 132
chris 0:78f9794620a3 133 // Compute the derivative
chris 0:78f9794620a3 134 derivative = current_pos_of_line - previous_pos_of_line;
chris 0:78f9794620a3 135
chris 0:78f9794620a3 136 // Compute the integral
chris 0:78f9794620a3 137 integral += proportional;
chris 0:78f9794620a3 138
chris 0:78f9794620a3 139 // Remember the last position.
chris 0:78f9794620a3 140 previous_pos_of_line = current_pos_of_line;
chris 0:78f9794620a3 141
chris 0:78f9794620a3 142 // Compute the power
chris 0:78f9794620a3 143 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
chris 0:78f9794620a3 144
chris 0:78f9794620a3 145 // Compute new speeds
chris 0:78f9794620a3 146 right = speed+power;
chris 0:78f9794620a3 147 left = speed-power;
chris 0:78f9794620a3 148
chris 0:78f9794620a3 149 // limit checks
chris 0:78f9794620a3 150 if (right < MIN)
chris 0:78f9794620a3 151 right = MIN;
chris 0:78f9794620a3 152 else if (right > MAX)
chris 0:78f9794620a3 153 right = MAX;
chris 0:78f9794620a3 154
chris 0:78f9794620a3 155 if (left < MIN)
chris 0:78f9794620a3 156 left = MIN;
chris 0:78f9794620a3 157 else if (left > MAX)
chris 0:78f9794620a3 158 left = MAX;
chris 0:78f9794620a3 159
chris 0:78f9794620a3 160 // set speed
chris 0:78f9794620a3 161 m3pi.left_motor(left);
chris 0:78f9794620a3 162 m3pi.right_motor(right);
Blunsdon 11:394ab193971f 163
janusboandersen 2:d22dcc2bfcc1 164 //Marc's sensor test
Blunsdon 12:dfc717775297 165 sensor_all(sensor_val); // sensor_value gets fkt value
Blunsdon 13:6a8689f601cd 166 if (sensor_val[0] > 350 && sensor_val[4] > 350)
Blunsdon 12:dfc717775297 167 {
Blunsdon 13:6a8689f601cd 168
Blunsdon 13:6a8689f601cd 169 m3pi.stop();
Blunsdon 13:6a8689f601cd 170 turn_right();
Blunsdon 13:6a8689f601cd 171 m3pi.stop();
Blunsdon 13:6a8689f601cd 172 m3pi.forward(0.2);
Blunsdon 16:48b81a9246e6 173 wait (1.1);
Blunsdon 14:408d9d087d0f 174 m3pi.stop();
Blunsdon 16:48b81a9246e6 175 wait (5);
Blunsdon 14:408d9d087d0f 176 m3pi.backward(0.2);
Blunsdon 15:fd6886ab896a 177 wait (1);
Blunsdon 14:408d9d087d0f 178 m3pi.stop();
Blunsdon 14:408d9d087d0f 179 turn_left();
Blunsdon 13:6a8689f601cd 180 m3pi.stop();
Blunsdon 12:dfc717775297 181 }
janusboandersen 5:527c046c7b8a 182 /*
janusboandersen 2:d22dcc2bfcc1 183 sensor_value4 = sensor_all(4); // sensor_value gets fkt value
janusboandersen 6:3be33adda32d 184 */
janusboandersen 7:30c63d6460f6 185
janusboandersen 6:3be33adda32d 186 /*
janusboandersen 2:d22dcc2bfcc1 187 m3pi.printf("%d", sensor_value0); // prints one of sidesensors value
janusboandersen 2:d22dcc2bfcc1 188 m3pi.locate(0,1);
janusboandersen 2:d22dcc2bfcc1 189 m3pi.printf("%d", sensor_value4); // prints the other sidesensor value
janusboandersen 3:47968be0df37 190 */
chris 0:78f9794620a3 191
chris 0:78f9794620a3 192 }
chris 0:78f9794620a3 193 }