├── Arduino ├── OpenMVArduino │ └── OpenMVArduino.ino ├── OpenMVArduinoKW │ └── OpenMVArduinoKW.ino └── OpenMVArduinoNew │ └── OpenMVArduinoNew.ino ├── ArduinoV2__sw_serial └── ArduinoV2__sw_serial_.ino ├── ColorlineMVP.py ├── DonkeyMV.py ├── DonkeyMV1.py ├── DonkeyMV2.py ├── LinearRegressionRC.py ├── LinearRegressionRC.py.autosave ├── MPVBlobs.py ├── MVPRacer.py ├── MVPRacerNewShield.py ├── MVR.py ├── OpenMVArduino └── OpenMVArduino.ino ├── OpenMVTeensy └── OpenMVTeensy.ino ├── README.md ├── Rover2020.py ├── ScanseRover.py ├── TeensyShield └── TeensyShield.ino ├── bwlinefollowingArduino.py ├── bwlinefollowingRC.py ├── colorLinearRegression.py ├── colorlinefollowing.py ├── colorlinefollowingRC.py ├── linefollowing.py ├── newcolor.py ├── pidrover.py ├── pulsein_test └── pulsein_test.ino ├── roslaunch-duckiebot-11119.log ├── rover.py └── rover2.py /Arduino/OpenMVArduino/OpenMVArduino.ino: -------------------------------------------------------------------------------- 1 | // This file is part of the OpenMV project. 2 | // Copyright (c) 2013-2017 Ibrahim Abdelkader & Kwabena W. Agyeman 3 | // This work is licensed under the MIT license, see the file LICENSE for details. 4 | 5 | #include 6 | 7 | #define SERIAL_RX_PIN 0 8 | #define SERIAL_TX_PIN 1 9 | #define THROTTLE_SERVO_PIN 10 10 | #define STEERING_SERVO_PIN 6 11 | #define RC_THROTTLE_SERVO_PIN 5 12 | #define RC_STEERING_SERVO_PIN 3 13 | 14 | #define SERIAL_BUAD_RATE 19200 15 | 16 | #define RC_THROTTLE_SERVO_REFRESH_RATE 20000UL // in us 17 | #define SERIAL_THROTTLE_SERVO_REFRESH_RATE 1000000UL // in us 18 | #define RC_THROTTLE_DEAD_ZONE_MIN 1400UL // in us 19 | #define RC_THROTTLE_DEAD_ZONE_MAX 1600UL // in us 20 | 21 | #define RC_STEERING_SERVO_REFRESH_RATE 20000UL // in us 22 | #define SERIAL_STEERING_SERVO_REFRESH_RATE 1000000UL // in us 23 | #define RC_STEERING_DEAD_ZONE_MIN 1400UL // in us 24 | #define RC_STEERING_DEAD_ZONE_MAX 1600UL // in us 25 | 26 | Servo throttle_servo, steering_servo; 27 | 28 | unsigned long last_microseconds; 29 | bool last_rc_throttle_pin_state, last_rc_steering_pin_state; 30 | unsigned long last_rc_throttle_microseconds, last_rc_steering_microseconds; 31 | unsigned long rc_throttle_servo_pulse_length = 0, rc_steering_servo_pulse_length = 0; 32 | unsigned long rc_throttle_servo_pulse_refreshed = 0, rc_steering_servo_pulse_refreshed = 0; 33 | 34 | char serial_buffer[16] = {}; 35 | unsigned long serial_throttle_servo_pulse_length = 0, serial_steering_servo_pulse_length = 0; 36 | unsigned long serial_throttle_servo_pulse_refreshed = 0, serial_steering_servo_pulse_refreshed = 0; 37 | 38 | void setup() 39 | { 40 | Serial.begin(SERIAL_BUAD_RATE); 41 | pinMode(LED_BUILTIN, OUTPUT); 42 | 43 | last_microseconds = micros(); 44 | last_rc_throttle_pin_state = digitalRead(RC_THROTTLE_SERVO_PIN) == HIGH; 45 | last_rc_steering_pin_state = digitalRead(RC_STEERING_SERVO_PIN) == HIGH; 46 | last_rc_throttle_microseconds = last_microseconds; 47 | last_rc_steering_microseconds = last_microseconds; 48 | } 49 | 50 | void loop() 51 | { 52 | unsigned long microseconds = micros(); 53 | bool rc_throttle_pin_state = digitalRead(RC_THROTTLE_SERVO_PIN) == HIGH; 54 | bool rc_steering_pin_state = digitalRead(RC_STEERING_SERVO_PIN) == HIGH; 55 | 56 | if(rc_throttle_pin_state && (!last_rc_throttle_pin_state)) // rising edge 57 | { 58 | last_rc_throttle_microseconds = microseconds; 59 | } 60 | 61 | if((!rc_throttle_pin_state) && last_rc_throttle_pin_state) // falling edge 62 | { 63 | unsigned long temp = microseconds - last_rc_throttle_microseconds; 64 | 65 | if(!rc_throttle_servo_pulse_length) 66 | { 67 | rc_throttle_servo_pulse_length = temp; 68 | } 69 | else 70 | { 71 | rc_throttle_servo_pulse_length = ((rc_throttle_servo_pulse_length * 3) + temp) >> 2; 72 | } 73 | 74 | rc_throttle_servo_pulse_refreshed = microseconds; 75 | } 76 | 77 | if(rc_throttle_servo_pulse_length // zero servo if not refreshed 78 | && ((microseconds - rc_throttle_servo_pulse_refreshed) > (2UL * RC_THROTTLE_SERVO_REFRESH_RATE))) 79 | { 80 | rc_throttle_servo_pulse_length = 0; 81 | } 82 | 83 | if(rc_steering_pin_state && (!last_rc_steering_pin_state)) // rising edge 84 | { 85 | last_rc_steering_microseconds = microseconds; 86 | } 87 | 88 | if((!rc_steering_pin_state) && last_rc_steering_pin_state) // falling edge 89 | { 90 | unsigned long temp = microseconds - last_rc_steering_microseconds; 91 | 92 | if(!rc_steering_servo_pulse_length) 93 | { 94 | rc_steering_servo_pulse_length = temp; 95 | } 96 | else 97 | { 98 | rc_steering_servo_pulse_length = ((rc_steering_servo_pulse_length * 3) + temp) >> 2; 99 | } 100 | 101 | rc_steering_servo_pulse_refreshed = microseconds; 102 | } 103 | 104 | if(rc_steering_servo_pulse_length // zero servo if not refreshed 105 | && ((microseconds - rc_steering_servo_pulse_refreshed) > (2UL * RC_STEERING_SERVO_REFRESH_RATE))) 106 | { 107 | rc_steering_servo_pulse_length = 0; 108 | } 109 | 110 | last_microseconds = microseconds; 111 | last_rc_throttle_pin_state = rc_throttle_pin_state; 112 | last_rc_steering_pin_state = rc_steering_pin_state; 113 | 114 | while(Serial.available()) 115 | { 116 | int c = Serial.read(); 117 | memmove(serial_buffer, serial_buffer + 1, sizeof(serial_buffer) - 2); 118 | serial_buffer[sizeof(serial_buffer) - 2] = c; 119 | 120 | if(c == '\n') 121 | { 122 | unsigned long serial_throttle_servo_pulse_length_tmp, serial_steering_servo_pulse_length_tmp; 123 | 124 | if(sscanf(serial_buffer, "{%lu,%lu}", &serial_throttle_servo_pulse_length_tmp, &serial_steering_servo_pulse_length_tmp) == 2) 125 | { 126 | if(!serial_throttle_servo_pulse_length) 127 | { 128 | serial_throttle_servo_pulse_length = serial_throttle_servo_pulse_length_tmp; 129 | } 130 | else 131 | { 132 | serial_throttle_servo_pulse_length = ((serial_throttle_servo_pulse_length * 3) + serial_throttle_servo_pulse_length_tmp) >> 2; 133 | } 134 | 135 | serial_throttle_servo_pulse_refreshed = microseconds; 136 | 137 | if(!serial_steering_servo_pulse_length) 138 | { 139 | serial_steering_servo_pulse_length = serial_steering_servo_pulse_length_tmp; 140 | } 141 | else 142 | { 143 | serial_steering_servo_pulse_length = ((serial_steering_servo_pulse_length * 3) + serial_steering_servo_pulse_length_tmp) >> 2; 144 | } 145 | 146 | serial_steering_servo_pulse_refreshed = microseconds; 147 | 148 | digitalWrite(LED_BUILTIN, (digitalRead(LED_BUILTIN) == HIGH) ? LOW : HIGH); 149 | } 150 | else 151 | { 152 | serial_throttle_servo_pulse_length = 0; 153 | serial_steering_servo_pulse_length = 0; 154 | } 155 | } 156 | } 157 | 158 | if(serial_throttle_servo_pulse_length // zero servo if not refreshed 159 | && ((microseconds - serial_throttle_servo_pulse_refreshed) > (2UL * SERIAL_THROTTLE_SERVO_REFRESH_RATE))) 160 | { 161 | serial_throttle_servo_pulse_length = 0; 162 | } 163 | 164 | if(serial_steering_servo_pulse_length // zero servo if not refreshed 165 | && ((microseconds - serial_steering_servo_pulse_refreshed) > (2UL * SERIAL_STEERING_SERVO_REFRESH_RATE))) 166 | { 167 | serial_steering_servo_pulse_length = 0; 168 | } 169 | 170 | if(rc_steering_servo_pulse_length) 171 | { 172 | if(!steering_servo.attached()) 173 | { 174 | throttle_servo.attach(THROTTLE_SERVO_PIN); 175 | steering_servo.attach(STEERING_SERVO_PIN); 176 | } 177 | 178 | if(serial_steering_servo_pulse_length) 179 | { 180 | if((rc_throttle_servo_pulse_length < RC_THROTTLE_DEAD_ZONE_MIN) 181 | || (rc_throttle_servo_pulse_length > RC_THROTTLE_DEAD_ZONE_MAX)) 182 | { 183 | throttle_servo.writeMicroseconds(serial_throttle_servo_pulse_length); 184 | } 185 | else 186 | { 187 | throttle_servo.writeMicroseconds(1500); 188 | } 189 | 190 | if((rc_steering_servo_pulse_length < RC_STEERING_DEAD_ZONE_MIN) 191 | || (rc_steering_servo_pulse_length > RC_STEERING_DEAD_ZONE_MAX)) 192 | { 193 | steering_servo.writeMicroseconds(rc_steering_servo_pulse_length); 194 | } 195 | else 196 | { 197 | steering_servo.writeMicroseconds(serial_steering_servo_pulse_length); 198 | } 199 | } 200 | else 201 | { 202 | throttle_servo.writeMicroseconds(rc_throttle_servo_pulse_length); 203 | steering_servo.writeMicroseconds(rc_steering_servo_pulse_length); 204 | } 205 | } 206 | else if(steering_servo.attached()) 207 | { 208 | throttle_servo.detach(); 209 | steering_servo.detach(); 210 | } 211 | } 212 | -------------------------------------------------------------------------------- /Arduino/OpenMVArduinoKW/OpenMVArduinoKW.ino: -------------------------------------------------------------------------------- 1 | // This file is part of the OpenMV project. 2 | // Copyright (c) 2013-2017 Ibrahim Abdelkader & Kwabena W. Agyeman 3 | // This work is licensed under the MIT license, see the file LICENSE for details. 4 | 5 | #include 6 | 7 | #define SERIAL_RX_PIN 0 8 | #define SERIAL_TX_PIN 1 9 | #define THROTTLE_SERVO_PIN 10 10 | #define STEERING_SERVO_PIN 6 11 | #define RC_THROTTLE_SERVO_PIN 5 12 | #define RC_STEERING_SERVO_PIN 3 13 | 14 | #define SERIAL_BUAD_RATE 19200 15 | 16 | #define RC_THROTTLE_SERVO_REFRESH_RATE 20000UL // in us 17 | #define SERIAL_THROTTLE_SERVO_REFRESH_RATE 1000000UL // in us 18 | #define RC_THROTTLE_DEAD_ZONE_MIN 1400UL // in us 19 | #define RC_THROTTLE_DEAD_ZONE_MAX 1600UL // in us 20 | 21 | #define RC_STEERING_SERVO_REFRESH_RATE 20000UL // in us 22 | #define SERIAL_STEERING_SERVO_REFRESH_RATE 1000000UL // in us 23 | #define RC_STEERING_DEAD_ZONE_MIN 1400UL // in us 24 | #define RC_STEERING_DEAD_ZONE_MAX 1600UL // in us 25 | 26 | Servo throttle_servo, steering_servo; 27 | 28 | unsigned long last_microseconds; 29 | bool last_rc_throttle_pin_state, last_rc_steering_pin_state; 30 | unsigned long last_rc_throttle_microseconds, last_rc_steering_microseconds; 31 | unsigned long rc_throttle_servo_pulse_length = 0, rc_steering_servo_pulse_length = 0; 32 | unsigned long rc_throttle_servo_pulse_refreshed = 0, rc_steering_servo_pulse_refreshed = 0; 33 | 34 | char serial_buffer[16] = {}; 35 | unsigned long serial_throttle_servo_pulse_length = 0, serial_steering_servo_pulse_length = 0; 36 | unsigned long serial_throttle_servo_pulse_refreshed = 0, serial_steering_servo_pulse_refreshed = 0; 37 | 38 | void setup() 39 | { 40 | Serial.begin(SERIAL_BUAD_RATE); 41 | pinMode(LED_BUILTIN, OUTPUT); 42 | 43 | last_microseconds = micros(); 44 | last_rc_throttle_pin_state = digitalRead(RC_THROTTLE_SERVO_PIN) == HIGH; 45 | last_rc_steering_pin_state = digitalRead(RC_STEERING_SERVO_PIN) == HIGH; 46 | last_rc_throttle_microseconds = last_microseconds; 47 | last_rc_steering_microseconds = last_microseconds; 48 | } 49 | 50 | void loop() 51 | { 52 | unsigned long microseconds = micros(); 53 | bool rc_throttle_pin_state = digitalRead(RC_THROTTLE_SERVO_PIN) == HIGH; 54 | bool rc_steering_pin_state = digitalRead(RC_STEERING_SERVO_PIN) == HIGH; 55 | 56 | if(rc_throttle_pin_state && (!last_rc_throttle_pin_state)) // rising edge 57 | { 58 | last_rc_throttle_microseconds = microseconds; 59 | } 60 | 61 | if((!rc_throttle_pin_state) && last_rc_throttle_pin_state) // falling edge 62 | { 63 | unsigned long temp = microseconds - last_rc_throttle_microseconds; 64 | 65 | if(!rc_throttle_servo_pulse_length) 66 | { 67 | rc_throttle_servo_pulse_length = temp; 68 | } 69 | else 70 | { 71 | rc_throttle_servo_pulse_length = ((rc_throttle_servo_pulse_length * 3) + temp) >> 2; 72 | } 73 | 74 | rc_throttle_servo_pulse_refreshed = microseconds; 75 | } 76 | 77 | if(rc_throttle_servo_pulse_length // zero servo if not refreshed 78 | && ((microseconds - rc_throttle_servo_pulse_refreshed) > (2UL * RC_THROTTLE_SERVO_REFRESH_RATE))) 79 | { 80 | rc_throttle_servo_pulse_length = 0; 81 | } 82 | 83 | if(rc_steering_pin_state && (!last_rc_steering_pin_state)) // rising edge 84 | { 85 | last_rc_steering_microseconds = microseconds; 86 | } 87 | 88 | if((!rc_steering_pin_state) && last_rc_steering_pin_state) // falling edge 89 | { 90 | unsigned long temp = microseconds - last_rc_steering_microseconds; 91 | 92 | if(!rc_steering_servo_pulse_length) 93 | { 94 | rc_steering_servo_pulse_length = temp; 95 | } 96 | else 97 | { 98 | rc_steering_servo_pulse_length = ((rc_steering_servo_pulse_length * 3) + temp) >> 2; 99 | } 100 | 101 | rc_steering_servo_pulse_refreshed = microseconds; 102 | } 103 | 104 | if(rc_steering_servo_pulse_length // zero servo if not refreshed 105 | && ((microseconds - rc_steering_servo_pulse_refreshed) > (2UL * RC_STEERING_SERVO_REFRESH_RATE))) 106 | { 107 | rc_steering_servo_pulse_length = 0; 108 | } 109 | 110 | last_microseconds = microseconds; 111 | last_rc_throttle_pin_state = rc_throttle_pin_state; 112 | last_rc_steering_pin_state = rc_steering_pin_state; 113 | 114 | while(Serial.available()) 115 | { 116 | int c = Serial.read(); 117 | memmove(serial_buffer, serial_buffer + 1, sizeof(serial_buffer) - 2); 118 | serial_buffer[sizeof(serial_buffer) - 2] = c; 119 | 120 | if(c == '\n') 121 | { 122 | unsigned long serial_throttle_servo_pulse_length_tmp, serial_steering_servo_pulse_length_tmp; 123 | 124 | if(sscanf(serial_buffer, "{%lu,%lu}", &serial_throttle_servo_pulse_length_tmp, &serial_steering_servo_pulse_length_tmp) == 2) 125 | { 126 | if(!serial_throttle_servo_pulse_length) 127 | { 128 | serial_throttle_servo_pulse_length = serial_throttle_servo_pulse_length_tmp; 129 | } 130 | else 131 | { 132 | serial_throttle_servo_pulse_length = ((serial_throttle_servo_pulse_length * 3) + serial_throttle_servo_pulse_length_tmp) >> 2; 133 | } 134 | 135 | serial_throttle_servo_pulse_refreshed = microseconds; 136 | 137 | if(!serial_steering_servo_pulse_length) 138 | { 139 | serial_steering_servo_pulse_length = serial_steering_servo_pulse_length_tmp; 140 | } 141 | else 142 | { 143 | serial_steering_servo_pulse_length = ((serial_steering_servo_pulse_length * 3) + serial_steering_servo_pulse_length_tmp) >> 2; 144 | } 145 | 146 | serial_steering_servo_pulse_refreshed = microseconds; 147 | 148 | digitalWrite(LED_BUILTIN, (digitalRead(LED_BUILTIN) == HIGH) ? LOW : HIGH); 149 | } 150 | else 151 | { 152 | serial_throttle_servo_pulse_length = 0; 153 | serial_steering_servo_pulse_length = 0; 154 | } 155 | } 156 | } 157 | 158 | if(serial_throttle_servo_pulse_length // zero servo if not refreshed 159 | && ((microseconds - serial_throttle_servo_pulse_refreshed) > (2UL * SERIAL_THROTTLE_SERVO_REFRESH_RATE))) 160 | { 161 | serial_throttle_servo_pulse_length = 0; 162 | } 163 | 164 | if(serial_steering_servo_pulse_length // zero servo if not refreshed 165 | && ((microseconds - serial_steering_servo_pulse_refreshed) > (2UL * SERIAL_STEERING_SERVO_REFRESH_RATE))) 166 | { 167 | serial_steering_servo_pulse_length = 0; 168 | } 169 | 170 | if(rc_steering_servo_pulse_length) 171 | { 172 | if(!steering_servo.attached()) 173 | { 174 | throttle_servo.attach(THROTTLE_SERVO_PIN); 175 | steering_servo.attach(STEERING_SERVO_PIN); 176 | } 177 | 178 | if(serial_steering_servo_pulse_length) 179 | { 180 | if((rc_throttle_servo_pulse_length < RC_THROTTLE_DEAD_ZONE_MIN) 181 | || (rc_throttle_servo_pulse_length > RC_THROTTLE_DEAD_ZONE_MAX)) 182 | { 183 | throttle_servo.writeMicroseconds(serial_throttle_servo_pulse_length); 184 | } 185 | else 186 | { 187 | throttle_servo.writeMicroseconds(1500); 188 | } 189 | 190 | if((rc_steering_servo_pulse_length < RC_STEERING_DEAD_ZONE_MIN) 191 | || (rc_steering_servo_pulse_length > RC_STEERING_DEAD_ZONE_MAX)) 192 | { 193 | steering_servo.writeMicroseconds(rc_steering_servo_pulse_length); 194 | } 195 | else 196 | { 197 | steering_servo.writeMicroseconds(serial_steering_servo_pulse_length); 198 | } 199 | } 200 | else 201 | { 202 | throttle_servo.writeMicroseconds(rc_throttle_servo_pulse_length); 203 | steering_servo.writeMicroseconds(rc_steering_servo_pulse_length); 204 | } 205 | } 206 | else if(steering_servo.attached()) 207 | { 208 | throttle_servo.detach(); 209 | steering_servo.detach(); 210 | } 211 | } 212 | -------------------------------------------------------------------------------- /Arduino/OpenMVArduinoNew/OpenMVArduinoNew.ino: -------------------------------------------------------------------------------- 1 | 2 | /* Miminimum sketch to translate incoming serial from an OpenMV board to Ardunio commands for motor and servo 3 | * Written by Chris Anderson, DIYRobocars, 2017 4 | */ 5 | 6 | #include //servo library 7 | #include 8 | 9 | #include 10 | 11 | AltSoftSerial altSerial; 12 | 13 | PWMServo myservoa, myservob; // create servo objects to control servos 14 | int SteeringPin = 10; // This corresponds to Servo Output 1 15 | int MotorPin = 6; // This correspondes to Servo Output 2 16 | int CH3 = 5; // Corresponds to Servo Output 3 17 | int CH4 = 4; // Corresponds to Servo Output 4 18 | int CH5 = 9; // Corresponds to Servo Outut 5 19 | int steer, motor; 20 | 21 | /* 22 | * Define pins used to provide RC PWM signal to Arduino 23 | * Pins 8, 9 and 10 are used since they work on both ATMega328 and 24 | * ATMega32u4 board. So this code will work on Uno/Mini/Nano/Micro/Leonardo 25 | * See PinChangeInterrupt documentation for usable pins on other boards 26 | */ 27 | const byte channel_pin[] = {8, 9, 10}; 28 | volatile unsigned long rising_start[] = {0, 0, 0}; 29 | volatile long channel_length[] = {0, 0, 0}; 30 | 31 | 32 | unsigned long time; 33 | unsigned long lasttime = 0; 34 | bool LEDState = LOW; 35 | 36 | const int BAUD_RATE = 9600; 37 | 38 | void setup() { 39 | Serial.begin(BAUD_RATE); 40 | myservoa.attach(SteeringPin);// attach servo on Output 1 to servo object 41 | myservob.attach(MotorPin);// attach servo on Output 2 to servo object 42 | altSerial.begin(9600); // This is for the debug console 43 | altSerial.println("Hello, world"); 44 | pinMode(LED_BUILTIN, OUTPUT); // enable LED 45 | pinMode(channel_pin[0], INPUT); 46 | pinMode(channel_pin[1], INPUT); 47 | pinMode(channel_pin[2], INPUT); 48 | 49 | attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(channel_pin[0]), onRising0, CHANGE); 50 | attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(channel_pin[1]), onRising1, CHANGE); 51 | attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(channel_pin[2]), onRising2, CHANGE); 52 | } 53 | 54 | void processPin(byte pin) { 55 | uint8_t trigger = getPinChangeInterruptTrigger(digitalPinToPCINT(channel_pin[pin])); 56 | 57 | if(trigger == RISING) { 58 | rising_start[pin] = micros(); 59 | } else if(trigger == FALLING) { 60 | channel_length[pin] = micros() - rising_start[pin]; 61 | } 62 | } 63 | 64 | void onRising0(void) { 65 | processPin(0); 66 | } 67 | 68 | void onRising1(void) { 69 | processPin(1); 70 | } 71 | 72 | void onRising2(void) { 73 | processPin(2); 74 | } 75 | void loop() { 76 | time = millis(); 77 | if (time > lasttime + 1000) { // flash the LED every second to show "resting" mode 78 | lasttime = time; 79 | LEDState = !LEDState; // reverse the LED state 80 | digitalWrite(LED_BUILTIN, LEDState); // turn on or off the LED 81 | } 82 | while (Serial.available() > 0) { 83 | // look for the next valid integer in the incoming serial stream: 84 | int tempsteer = Serial.parseInt(); 85 | // mySerial.println(steer); // turn this on for debgging. Warning: it will make the servos pulse 86 | // do it again: 87 | int tempmotor = Serial.parseInt(); 88 | // mySerial.println(motor); // turn this on for debgging. Warning: it will make the servos pulse 89 | // look for the newline. That's the end of the commands 90 | if (Serial.read() == '\n') { 91 | steer=tempsteer; 92 | motor=tempmotor; 93 | LEDState = !LEDState; // reverse the LED state 94 | digitalWrite(LED_BUILTIN, LEDState); // turn on or off the LED to show activity 95 | } 96 | } 97 | myservoa.write(steer); // send values to output 98 | myservob.write(motor); 99 | delay (20); 100 | } 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /ArduinoV2__sw_serial/ArduinoV2__sw_serial_.ino: -------------------------------------------------------------------------------- 1 | 2 | /* Miminimum sketch to translate incoming serial from an OpenMV board to Ardunio commands for motor and servo 3 | * Written by Chris Anderson, DIYRobocars, 2017 4 | */ 5 | #include // use this rather than the standard Servo to avoid interrupt clashes with SoftwareSerial 6 | #include 7 | 8 | SoftwareSerial mySerial(5, 4); // RX, TX 9 | PWMServo myservoa, myservob, myservoc, myservod, myservoe; // create servo objects to control servos 10 | int SteeringPin = 10; // This corresponds to Servo Output 1 11 | int MotorPin = 6; // This correspondes to Servo Output 2 12 | int CH3 = 5; // Corresponds to Servo Output 3 13 | int CH4 = 4; // Corresponds to Servo Output 4 14 | int CH5 = 9; // Corresponds to Servo Outut 5 15 | int steer, motor; 16 | 17 | unsigned long time; 18 | unsigned long lasttime = 0; 19 | bool LEDState = LOW; 20 | 21 | const int BAUD_RATE = 9600; 22 | 23 | void setup() { 24 | mySerial.begin(BAUD_RATE); 25 | myservoa.attach(SteeringPin);// attach servo on Output 1 to servo object 26 | myservob.attach(MotorPin);// attach servo on Output 2 to servo object 27 | Serial.begin(BAUD_RATE); // For debugging 28 | Serial.println("Hello, world"); // For debugging 29 | pinMode(LED_BUILTIN, OUTPUT); // enable LED 30 | } 31 | 32 | void loop() { 33 | time = millis(); 34 | if (time > lasttime + 1000) { // flash the LED every second to show "resting" mode 35 | lasttime = time; 36 | LEDState = !LEDState; // reverse the LED state 37 | digitalWrite(LED_BUILTIN, LEDState); // turn on or off the LED 38 | } 39 | while (mySerial.available() > 0) { 40 | // look for the next valid integer in the incoming serial stream: 41 | int tempsteer = mySerial.parseInt(); 42 | Serial.println(steer); // turn this on for debgging. 43 | // do it again: 44 | int tempmotor = mySerial.parseInt(); 45 | Serial.println(motor); // turn this on for debgging. 46 | // look for the newline. That's the end of the commands 47 | if (mySerial.read() == '\n') { 48 | steer=tempsteer; 49 | motor=tempmotor; 50 | LEDState = !LEDState; // reverse the LED state 51 | digitalWrite(LED_BUILTIN, LEDState); // turn on or off the LED to show activity 52 | } 53 | } 54 | myservoa.write(steer); // send values to output 55 | myservob.write(motor); 56 | delay (20); 57 | } 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /ColorlineMVP.py: -------------------------------------------------------------------------------- 1 | # Color Line Following Example with PID Steering 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import Servo 10 | from pyb import LED 11 | from pyb import UART 12 | uart = UART(3, 9600) # no need to go faster than this. Slower = solid comms 13 | 14 | # Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max) 15 | # The below thresholds track in general red/green things. You may wish to tune them... 16 | # old thresholds = [(30, 100, 15, 127, 15, 127), # generic_red_thresholds 17 | # (30, 100, -64, -8, -32, 32), # generic_green_thresholds 18 | # (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds 19 | 20 | threshold_index = 3 21 | # 0 for red, 1 for green, 2 for blue 22 | 23 | thresholds = [(30, 100, 15, 127, 15, 127), # generic_red_thresholds 24 | (0, 83, -128, 15, -128, 127), # generic_green_thresholds 25 | (0, 100, -128, -10, -128, 51), # generic_blue_thresholds 26 | (0, 100, -47, 127, 14, 127)] # generic yellow threshold 27 | # You may pass up to 16 thresholds above. However, it's not really possible to segment any 28 | # scene with 16 thresholds before color thresholds start to overlap heavily. 29 | 30 | 31 | cruise_speed = 0 # how fast should the car drive, range from 1000 to 2000 32 | steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction 33 | steering_gain = 1.1 # calibration for your car's steering sensitivity 34 | steering_center = 80 # set to your car servo's center point 35 | kp = 0.4 # P term of the PID 36 | ki = 0.0 # I term of the PID 37 | kd = 0.3 # D term of the PID 38 | 39 | red_led = LED(1) 40 | green_led = LED(2) 41 | blue_led = LED(3) 42 | ir_led = LED(4) 43 | 44 | old_error = 0 45 | measured_angle = 0 46 | set_angle = 90 # this is the desired steering angle (straight ahead) 47 | p_term = 0 48 | i_term = 0 49 | d_term = 0 50 | old_time = pyb.millis() 51 | 52 | def led_control(x): 53 | if (x&1)==0: red_led.off() 54 | elif (x&1)==1: red_led.on() 55 | if (x&2)==0: green_led.off() 56 | elif (x&2)==2: green_led.on() 57 | if (x&4)==0: blue_led.off() 58 | elif (x&4)==4: blue_led.on() 59 | if (x&8)==0: ir_led.off() 60 | elif (x&8)==8: ir_led.on() 61 | 62 | def constrain(value, min, max): 63 | if value < min : 64 | return min 65 | if value > max : 66 | return max 67 | else: 68 | return value 69 | 70 | def steer(angle): 71 | global steering_gain, cruise_speed, steering_center 72 | angle = int(round((angle+steering_center)*steering_gain)) 73 | constrain(angle, 0, 180) 74 | 75 | 76 | def update_pid(): 77 | global old_time, old_error, measured_angle, set_angle 78 | global p_term, i_term, d_term 79 | now = pyb.millis() 80 | dt = now - old_time 81 | error = set_angle - measured_angle 82 | de = error - old_error 83 | 84 | p_term = kp * error 85 | i_term += ki * error 86 | i_term = constrain(i_term, 0, 100) 87 | d_term = (de / dt) * kd 88 | 89 | old_error = error 90 | output = steering_direction * (p_term + i_term + d_term) 91 | output = constrain(output, -50, 50) 92 | return output 93 | 94 | 95 | # Each roi is (x, y, w, h). The line detection algorithm will try to find the 96 | # centroid of the largest blob in each roi. The x position of the centroids 97 | # will then be averaged with different weights where the most weight is assigned 98 | # to the roi near the bottom of the image and less to the next roi and so on. 99 | ROIS = [ # [ROI, weight] 100 | (0, 100, 160, 20, 0.1), # You'll need to tweak the weights for your app 101 | (0, 050, 160, 20, 0.3), # depending on how your robot is setup. 102 | (0, 000, 160, 20, 0.7) 103 | ] 104 | 105 | 106 | # Compute the weight divisor (we're computing this so you don't have to make weights add to 1). 107 | weight_sum = 0 108 | for r in ROIS: weight_sum += r[4] # r[4] is the roi weight. 109 | 110 | # Camera setup... 111 | clock = time.clock() # Tracks FPS. 112 | sensor.reset() # Initialize the camera sensor. 113 | sensor.__write_reg(0x6B, 0x22) # switches camera into advanced calibration mode. See this for more: http://forums.openmv.io/viewtopic.php?p=1358#p1358 114 | sensor.set_pixformat(sensor.RGB565) 115 | sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed. 116 | sensor.set_vflip(True) 117 | sensor.set_hmirror(True) 118 | sensor.set_auto_gain(True) # do some calibration at the start 119 | sensor.set_auto_whitebal(True) 120 | sensor.skip_frames(60) # Let new settings take effect. 121 | sensor.set_auto_gain(False) # now turn off autocalibration before we start color tracking 122 | sensor.set_auto_whitebal(False) 123 | 124 | 125 | 126 | while(True): 127 | clock.tick() # Track elapsed milliseconds between snapshots(). 128 | img = sensor.snapshot() # Take a picture and return the image. 129 | centroid_sum = 0 130 | for r in ROIS: 131 | blobs = img.find_blobs([thresholds[threshold_index]], roi=r[0:4], merge=True) # r[0:4] is roi tuple. 132 | if blobs: 133 | # Find the index of the blob with the most pixels. 134 | most_pixels = 0 135 | largest_blob = 0 136 | for i in range(len(blobs)): 137 | if blobs[i].pixels() > most_pixels: 138 | most_pixels = blobs[i].pixels() 139 | largest_blob = i 140 | 141 | # Draw a rect around the blob. 142 | img.draw_rectangle(blobs[largest_blob].rect()) 143 | img.draw_cross(blobs[largest_blob].cx(), 144 | blobs[largest_blob].cy()) 145 | 146 | centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight. 147 | 148 | center_pos = (centroid_sum / weight_sum) # Determine center of line. 149 | 150 | # Convert the center_pos to a deflection angle. We're using a non-linear 151 | # operation so that the response gets stronger the farther off the line we 152 | # are. Non-linear operations are good to use on the output of algorithms 153 | # like this to cause a response "trigger". 154 | deflection_angle = 0 155 | # The 80 is from half the X res, the 60 is from half the Y res. The 156 | # equation below is just computing the angle of a triangle where the 157 | # opposite side of the triangle is the deviation of the center position 158 | # from the center and the adjacent side is half the Y res. This limits 159 | # the angle output to around -45 to 45. (It's not quite -45 and 45). 160 | deflection_angle = -math.atan((center_pos-80)/60) 161 | 162 | # Convert angle in radians to degrees. 163 | deflection_angle = math.degrees(deflection_angle) 164 | 165 | # Now you have an angle telling you how much to turn the robot by which 166 | # incorporates the part of the line nearest to the robot and parts of 167 | # the line farther away from the robot for a better prediction. 168 | # print("Turn Angle: %f" % deflection_angle) 169 | now = pyb.millis() 170 | if now > old_time + 1.0 : # time has passed since last measurement 171 | measured_angle = deflection_angle + 90 172 | steer_angle = update_pid() 173 | old_time = now 174 | steer (steer_angle) 175 | print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle)) 176 | -------------------------------------------------------------------------------- /DonkeyMV.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | import time 5 | from rplidar import RPLidar 6 | import math 7 | import serial 8 | 9 | 10 | 11 | angle_offset = 0 # this compensates for the Lidar being placed in a rotated position 12 | gain = 1.5 # this is the steering gain. The PWM output to the steering servo must be between 0 (left) and 200 (right) 13 | speed = 1000 # crusing speed, must be between 0 and 3600 14 | steering_correction = -10 # this compensates for any steering bias the car has. Positive numbers steer to the right 15 | start = time.time() 16 | stop = False 17 | 18 | 19 | PORT_NAME = '/dev/ttyUSB0' # this is for the Lidar 20 | Teensy_Port = serial.Serial("/dev/ttyACM0", baudrate=19200, timeout=1) 21 | 22 | def constrain(val, min_val, max_val): 23 | if val < min_val: return min_val 24 | if val > max_val: return max_val 25 | return val 26 | 27 | 28 | # def steer(angle): 29 | # angle = 100 + gain*angle 30 | # angle = int(constrain(angle,0,300)) 31 | # print (angle) 32 | 33 | 34 | def scan(lidar): 35 | global stop 36 | point_list = [] 37 | slope_list = [] 38 | while True: 39 | print('Recording measurements... Press Crl+C to stop.') 40 | lasttime = time.time() 41 | for measurment in lidar.iter_measurments(): 42 | if stop == True: 43 | lidar.stop() 44 | lidar.stop_motor() 45 | lidar.disconnect() 46 | break 47 | if time.time() < (lasttime + 0.1): 48 | # [0] -> New 49 | # [1] -> Quality 50 | # [2] -> Angle (0 - 360) 51 | # [3] -> Distance in mm 52 | #print("time: ", time.time()) 53 | #print("lasttime: ", lasttime) 54 | if((measurment[3] > 100) and (measurment[3]<2000) and (measurment[1]>2) and ((measurment[2] < 135) or (225 < measurment[2]))): 55 | x_pos = math.cos(math.radians(measurment[2])) * measurment[3] # in mm 56 | y_pos = math.sin(math.radians(measurment[2])) * measurment[3] # in mm 57 | point_list.append((x_pos, y_pos)) 58 | else: 59 | if len(point_list) > 2: 60 | for i in range(len(point_list)): 61 | this_point = point_list[i] 62 | for j in range(len(point_list) - 1 - i): 63 | other_point = point_list[1 + i] 64 | mx = this_point[0] - other_point[0] 65 | my = this_point[1] - other_point[1] 66 | temp = math.atan2(my,mx) 67 | if not math.isnan(temp) and not math.isinf(temp): 68 | slope_list.append(temp) 69 | slope_list = sorted(slope_list) 70 | median = math.degrees(slope_list[len(slope_list)/2]) % 180 71 | if median < 0: median += 180 72 | #median += 90 73 | median = math.tan(math.radians(median)) 74 | Teensy_Port.write("%f\n" % median) 75 | print(median) 76 | point_list = [] 77 | slope_list = [] 78 | lasttime = time.time() 79 | 80 | ''' 81 | 82 | 83 | if measurment[1] > 2 and ((measurment[2] > 300 or measurment[2] < 60)): # in angular range 84 | if (measurment[3] < 1000 and measurment[3] > 100): # in distance range 85 | # print (measurment[2]) 86 | if (measurment[2] < 60): 87 | temp = measurment[2] 88 | else: 89 | temp = -1* (360-measurment[2]) # convert to negative angle to the left of center 90 | data = data + temp # sum of the detected angles, so we can average later 91 | # range_sum = range_sum + measurment[3] # sum all the distances so we can normalize later 92 | counter = counter + 1 # increment counter 93 | if time.time() > (lasttime + 0.1): 94 | # print("this should happen ten times a second") 95 | if counter > 0: # this means we see something 96 | average_angle = (data/counter) - angle_offset # average of detected angles 97 | print ("Average angle: ", average_angle) 98 | Teensy_Port.write("%f\n" % average_angle) 99 | obstacle_direction = int(100*math.atan(math.radians(average_angle))) # convert to a vector component 100 | drive_direction = -1 * obstacle_direction # steer in the opposite direction as obstacle (I'll replace this with a PID) 101 | # print ("Drive direction: ", drive_direction) 102 | counter = 0 # reset counter 103 | data = 0 # reset data 104 | range_sum = 0 105 | else: 106 | drive_direction = 0 107 | # steer(drive_direction) # Send data to motors 108 | lasttime = time.time() # reset 10Hz timer 109 | 110 | ''' 111 | 112 | def run(): 113 | '''Main function''' 114 | lidar = RPLidar(PORT_NAME) 115 | lidar.start_motor() 116 | time.sleep(1) 117 | info = lidar.get_info() 118 | print(info) 119 | try: 120 | scan(lidar) 121 | except KeyboardInterrupt: 122 | stop = True 123 | print('Stopping.') 124 | lidar.stop() 125 | lidar.stop_motor() 126 | lidar.disconnect() 127 | 128 | if __name__ == '__main__': 129 | run() 130 | -------------------------------------------------------------------------------- /DonkeyMV1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | import time 5 | from rplidar import RPLidar 6 | import math 7 | import serial 8 | 9 | 10 | 11 | angle_offset = 0 # this compensates for the Lidar being placed in a rotated position 12 | gain = 1.5 # this is the steering gain. The PWM output to the steering servo must be between 0 (left) and 200 (right) 13 | speed = 1000 # crusing speed, must be between 0 and 3600 14 | steering_correction = -10 # this compensates for any steering bias the car has. Positive numbers steer to the right 15 | start = time.time() 16 | stop = False 17 | 18 | 19 | PORT_NAME = '/dev/ttyUSB0' # this is for the Lidar 20 | Teensy_Port = serial.Serial("/dev/ttyACM0", baudrate=19200, timeout=1) 21 | 22 | def constrain(val, min_val, max_val): 23 | if val < min_val: return min_val 24 | if val > max_val: return max_val 25 | return val 26 | 27 | 28 | # def steer(angle): 29 | # angle = 100 + gain*angle 30 | # angle = int(constrain(angle,0,300)) 31 | # print (angle) 32 | 33 | 34 | def scan(lidar): 35 | while True: 36 | print('Recording measurements... Press Crl+C to stop.') 37 | counter = 0 38 | data = 0 39 | lasttime = time.time() 40 | for measurment in lidar.iter_measurments(): 41 | if time.time() < (lasttime + 0.1): 42 | if measurment[1] > 2 and ((measurment[2] > 300 or measurment[2] < 60)): # in angular [2] and quality [1] range 43 | if (measurment[3] < 1000 and measurment[3] > 100): # in distance range 44 | # print (measurment[2]) 45 | if (measurment[2] < 60): 46 | temp = measurment[2] 47 | else: 48 | temp = -1* (360-measurment[2]) # convert to negative angle to the left of center 49 | data = data + temp # sum of the detected angles, so we can average later 50 | # range_sum = range_sum + measurment[3] # sum all the distances so we can normalize later 51 | counter = counter + 1 # increment counter 52 | else: # 10Hz timer has expired 53 | # print("this should happen ten times a second") 54 | if counter > 0: # this means we see something 55 | average_angle = -1 * ((data/counter) - angle_offset) # average of detected angles 56 | print ("Average angle: ", average_angle) 57 | Teensy_Port.write("%f\n" % average_angle) 58 | obstacle_direction = int(100*math.atan(math.radians(average_angle))) # convert to a vector component 59 | drive_direction = -1 * obstacle_direction # steer in the opposite direction as obstacle (I'll replace this with a PID) 60 | # print ("Drive direction: ", drive_direction) 61 | counter = 0 # reset counter 62 | data = 0 # reset data 63 | range_sum = 0 64 | else: 65 | drive_direction = 0 66 | # steer(drive_direction) # Send data to motors 67 | lasttime = time.time() # reset 10Hz timer 68 | 69 | 70 | def run(): 71 | '''Main function''' 72 | lidar = RPLidar(PORT_NAME) 73 | lidar.start_motor() 74 | time.sleep(1) 75 | info = lidar.get_info() 76 | print(info) 77 | try: 78 | scan(lidar) 79 | except KeyboardInterrupt: 80 | stop = True 81 | print('Stopping.') 82 | lidar.stop() 83 | lidar.stop_motor() 84 | lidar.disconnect() 85 | 86 | if __name__ == '__main__': 87 | run() 88 | -------------------------------------------------------------------------------- /DonkeyMV2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | import time 5 | from rplidar import RPLidar 6 | import math 7 | import serial 8 | 9 | 10 | 11 | angle_offset = 0 # this compensates for the Lidar being placed in a rotated position 12 | start = time.time() 13 | stop = False 14 | 15 | 16 | PORT_NAME = '/dev/ttyUSB0' # this is for the Lidar 17 | Teensy_Port = serial.Serial("/dev/ttyACM0", baudrate=19200, timeout=1) 18 | 19 | def constrain(val, min_val, max_val): 20 | if val < min_val: return min_val 21 | if val > max_val: return max_val 22 | return val 23 | 24 | 25 | def scan(lidar): 26 | global stop 27 | point_list = [] 28 | slope_list = [] 29 | while True: 30 | print('Recording measurements... Press Crl+C to stop.') 31 | lasttime = time.time() 32 | for measurment in lidar.iter_measurments(): 33 | if stop == True: 34 | lidar.stop() 35 | lidar.stop_motor() 36 | lidar.disconnect() 37 | break 38 | if time.time() < (lasttime + 0.1): 39 | # [0] -> New 40 | # [1] -> Quality 41 | # [2] -> Angle (0 - 360) 42 | # [3] -> Distance in mm 43 | #print("time: ", time.time()) 44 | #print("lasttime: ", lasttime) 45 | if((measurment[3] > 100) and (measurment[3]<2500) and (measurment[1]>2) and ((measurment[2] < 135) or (225 < measurment[2]))): 46 | x_pos = math.cos(math.radians(measurment[2])) * measurment[3] # in mm 47 | y_pos = math.sin(math.radians(measurment[2])) * measurment[3] # in mm 48 | point_list.append((x_pos, y_pos)) 49 | else: 50 | if len(point_list) > 2: 51 | for i in range(len(point_list)): 52 | this_point = point_list[i] 53 | for j in range(len(point_list) - 1 - i): 54 | other_point = point_list[1 + i] 55 | mx = this_point[0] - other_point[0] 56 | my = this_point[1] - other_point[1] 57 | temp = math.atan2(my,mx) 58 | if not math.isnan(temp) and not math.isinf(temp): 59 | slope_list.append(temp) 60 | slope_list = sorted(slope_list) 61 | median = math.degrees(slope_list[len(slope_list)/2]) 62 | median = median % 180 # forcing output to be between 0 and 179 63 | if median < 0: median += 180 64 | 65 | slope_sum = 0 66 | for j in range(len(slope_list)): 67 | temp2 = slope_list[j] % 180 68 | if temp2 < 0: 69 | temp2 += 180 70 | temp2 = temp2 - median 71 | temp2 = temp2*temp2 72 | slope_sum = slope_sum + temp2 73 | slope_sum = slope_sum / len(slope_list) 74 | slope_sum = math.sqrt(slope_sum) 75 | # print("Slope sum: ",slope_sum) 76 | median = 10 * (1/math.tan(math.radians(median))) 77 | Teensy_Port.write("%f\n" % median) 78 | print(median) 79 | point_list = [] 80 | slope_list = [] 81 | lasttime = time.time() 82 | 83 | def run(): 84 | '''Main function''' 85 | lidar = RPLidar(PORT_NAME) 86 | lidar.start_motor() 87 | time.sleep(1) 88 | info = lidar.get_info() 89 | print(info) 90 | try: 91 | scan(lidar) 92 | except KeyboardInterrupt: 93 | stop = True 94 | print('Stopping.') 95 | lidar.stop() 96 | lidar.stop_motor() 97 | lidar.disconnect() 98 | 99 | if __name__ == '__main__': 100 | run() 101 | -------------------------------------------------------------------------------- /LinearRegressionRC.py: -------------------------------------------------------------------------------- 1 | # Color Line Following Example with PID Steering 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import Servo 10 | from pyb import LED 11 | from pyb import UART 12 | from pyb import Pin 13 | 14 | 15 | switch_pin = Pin('P7', Pin.IN, Pin.PULL_DOWN) 16 | 17 | uart = UART(3, 19200) # Use UART 3 (pins 4 and 5) 18 | 19 | blue_threshold = (0, 100, -128, 127, -128, -35) # L A B 20 | 21 | binary_threshold = (57, 255) 22 | 23 | rois = [0,40,320,190] 24 | 25 | TARGET_POINTS = [(70, 50), # (x, y), clockwise from top left 26 | (285, 50), 27 | (319, 239), 28 | (1,239)] 29 | 30 | 31 | cruise_speed = 1575 # how fast should the car drive, range from 1000 (full backwards) to 2000 (full forwards). 1500 is stopped. 1575 is slowest it can go 32 | steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction 33 | steering_gain = 30 # calibration for your car's steering sensitivity 34 | steering_center = -5 # set to your car servo's center point 35 | 36 | 37 | kp = 0.4 # P term of the PID 38 | ki = 0.0 # I term of the PID 39 | kd = 0.3 # D term of the PID 40 | 41 | i_term = 0 42 | 43 | old_error = 0 44 | measured_angle = 0 45 | set_angle = 0 # this is the desired steering angle (straight ahead) 46 | old_time = pyb.millis() 47 | led_time = pyb.millis() 48 | 49 | 50 | 51 | RC_control = True # Set to false if you're just benchtop testing 52 | 53 | red_led = LED(1) 54 | green_led = LED(2) 55 | blue_led = LED(3) 56 | ir_led = LED(4) 57 | led_state = True 58 | 59 | 60 | 61 | def led_control(x): 62 | if (x&1)==0: red_led.off() 63 | elif (x&1)==1: red_led.on() 64 | if (x&2)==0: green_led.off() 65 | elif (x&2)==2: green_led.on() 66 | if (x&4)==0: blue_led.off() 67 | elif (x&4)==4: blue_led.on() 68 | if (x&8)==0: ir_led.off() 69 | elif (x&8)==8: ir_led.on() 70 | 71 | def constrain(value, min, max): 72 | if value < min : 73 | return min 74 | if value > max : 75 | return max 76 | else: 77 | return value 78 | 79 | def steer(angle): 80 | angle = int(round((angle+steering_center)*steering_gain)) 81 | ch1 = str(angle) # must convert to text to send via Serial 82 | ch2 = str(cruise_speed) # send throttle data, too 83 | print("Steering command", angle) 84 | uart.write(ch1) # send to the Arduino. It looks for channel outputs in order, seperated by a "," and ended with a "\n" 85 | uart.write(",") 86 | uart.write(ch2) 87 | uart.write("\n") 88 | 89 | def update_pid(measured_angle): 90 | global old_error, i_term 91 | now = pyb.millis() 92 | dt = now - old_time 93 | error = set_angle - measured_angle 94 | de = error - old_error 95 | 96 | p_term = kp * error 97 | i_term += ki * error 98 | i_term = constrain(i_term, 0, 100) 99 | d_term = (de / dt) * kd 100 | 101 | old_error = error 102 | output = steering_direction * (p_term + i_term + d_term) 103 | output = constrain(output, -50, 50) 104 | return output 105 | 106 | 107 | 108 | # Camera setup... 109 | clock = time.clock() # Tracks FPS. 110 | sensor.reset() # Initialize the camera sensor. 111 | sensor.__write_reg(0x6B, 0x22) # switches camera into advanced calibration mode. See this for more: http://forums.openmv.io/viewtopic.php?p=1358#p1358 112 | sensor.set_pixformat(sensor.RGB565) 113 | sensor.set_framesize(sensor.QVGA) # use QVGA for speed. 114 | sensor.set_vflip(True) 115 | sensor.set_auto_gain(True) # do some calibration at the start 116 | sensor.set_auto_whitebal(True) 117 | sensor.skip_frames(60) # Let new settings take effect. 118 | sensor.set_auto_gain(False) # now turn off autocalibration before we start color tracking 119 | sensor.set_auto_whitebal(False) 120 | 121 | 122 | while(True): 123 | clock.tick() # Track elapsed milliseconds between snapshots(). 124 | switch = switch_pin.value() # get value, 0 or 1 125 | if switch == 1 or not RC_control: # Teensy says you're in MV mode 126 | img = sensor.snapshot().lens_corr(strength = 2.8, zoom = 1) 127 | img.binary([blue_threshold]) 128 | line = img.get_regression([binary_threshold],roi=rois, robust = False) # do linear regression for desired color 129 | if (line): 130 | img.draw_line(line.line(), color = 127) 131 | offset = line.line()[2] 132 | constrain(offset,0,320) 133 | offset = offset - 160 134 | offset_angle = math.acos(offset/320) 135 | offset_angle = math.degrees(offset_angle) - 90 136 | measured_angle = -math.atan2(line.line()[1]-line.line()[3],line.line()[0]-line.line()[2]) 137 | print(line.line()) 138 | # Convert angle in radians to degrees. 139 | measured_angle = math.degrees(measured_angle) 140 | measured_angle = 90 - measured_angle # make straight ahead 0 141 | measured_angle = constrain(measured_angle, -90, 90) 142 | net_angle = 2*offset_angle - measured_angle 143 | # print("Offset Angle: ", offset_angle, "Measured Angle: ", measured_angle, "Net Angle: ", net_angle) 144 | 145 | # print("FPS %f" % clock.fps()) 146 | 147 | 148 | 149 | now = pyb.millis() 150 | if now > old_time + 0.02 : # time has passed since last measurement; do PID at 50Hz 151 | if now > led_time + 1000: # switch LED every second 152 | if led_state == True: 153 | led_control(0) # turn off LED 154 | led_state = False 155 | else: 156 | led_control(2) # turn on Green LED 157 | led_state = True 158 | led_time = now # reset time counter 159 | steer_angle = update_pid(net_angle) 160 | old_time = now 161 | steer (steer_angle) 162 | 163 | else: 164 | now = pyb.millis() 165 | if now > old_time + 1.0 : # time has passed since last measurement 166 | if now > led_time + 1000: # switch LED every second 167 | print("Under RC control") 168 | if led_state == True: 169 | led_control(0) # turn off LED 170 | led_state = False 171 | else: 172 | led_control(4) # turn on Blue LED 173 | led_state = True 174 | led_time = now # reset time counter 175 | 176 | -------------------------------------------------------------------------------- /LinearRegressionRC.py.autosave: -------------------------------------------------------------------------------- 1 | # Color Line Following Example with PID Steering 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import LED 10 | from pyb import UART 11 | from pyb import Pin 12 | 13 | 14 | RC_control = False # Set to false if you're just benchtop testing 15 | 16 | switch_pin = Pin('P7', Pin.IN, Pin.PULL_DOWN) 17 | 18 | uart = UART(3, 19200) # Use UART 3 (pins 4 and 5) 19 | 20 | #blue_threshold = (0, 100, -128, 127, -128, -35) # lights 21 | blue_threshold = (37, 100, -128, 8, -128, -29) # sunlight 22 | 23 | binary_threshold = (57, 255) 24 | 25 | rois = [0,40,320,190] 26 | 27 | TARGET_POINTS = [(70, 50), # (x, y), clockwise from top left 28 | (285, 50), 29 | (319, 239), 30 | (1,239)] 31 | 32 | 33 | cruise_speed = 1575 # how fast should the car drive, range from 1000 (full backwards) to 2000 (full forwards). 1500 is stopped. 1575 is slowest it can go 34 | steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction 35 | steering_gain = 30 # calibration for your car's steering sensitivity 36 | steering_center = -5 # set to your car servo's center point 37 | 38 | 39 | kp = 0.4 # P term of the PID 40 | ki = 0.0 # I term of the PID 41 | kd = 0.3 # D term of the PID 42 | 43 | i_term = 0 44 | 45 | old_error = 0 46 | measured_angle = 0 47 | set_angle = 0 # this is the desired steering angle (straight ahead) 48 | old_time = pyb.millis() 49 | led_time = pyb.millis() 50 | 51 | 52 | 53 | 54 | 55 | red_led = LED(1) 56 | green_led = LED(2) 57 | blue_led = LED(3) 58 | ir_led = LED(4) 59 | led_state = True 60 | 61 | 62 | 63 | def led_control(x): 64 | if (x&1)==0: red_led.off() 65 | elif (x&1)==1: red_led.on() 66 | if (x&2)==0: green_led.off() 67 | elif (x&2)==2: green_led.on() 68 | if (x&4)==0: blue_led.off() 69 | elif (x&4)==4: blue_led.on() 70 | if (x&8)==0: ir_led.off() 71 | elif (x&8)==8: ir_led.on() 72 | 73 | def constrain(value, min, max): 74 | if value < min : 75 | return min 76 | if value > max : 77 | return max 78 | else: 79 | return value 80 | 81 | def steer(angle): 82 | angle = int(round((angle+steering_center)*steering_gain)) 83 | ch1 = str(angle) # must convert to text to send via Serial 84 | ch2 = str(cruise_speed) # send throttle data, too 85 | print("Steering command", angle) 86 | uart.write(ch1) # send to the Arduino. It looks for channel outputs in order, seperated by a "," and ended with a "\n" 87 | uart.write(",") 88 | uart.write(ch2) 89 | uart.write("\n") 90 | 91 | def update_pid(measured_angle): 92 | global old_error, i_term 93 | now = pyb.millis() 94 | dt = now - old_time 95 | error = set_angle - measured_angle 96 | de = error - old_error 97 | 98 | p_term = kp * error 99 | i_term += ki * error 100 | i_term = constrain(i_term, 0, 100) 101 | d_term = (de / dt) * kd 102 | 103 | old_error = error 104 | output = steering_direction * (p_term + i_term + d_term) 105 | output = constrain(output, -50, 50) 106 | return output 107 | 108 | 109 | 110 | # Camera setup... 111 | clock = time.clock() # Tracks FPS. 112 | sensor.reset() # Initialize the camera sensor. 113 | sensor.__write_reg(0x6B, 0x22) # switches camera into advanced calibration mode. See this for more: http://forums.openmv.io/viewtopic.php?p=1358#p1358 114 | sensor.set_pixformat(sensor.RGB565) 115 | sensor.set_framesize(sensor.QVGA) # use QVGA for speed. 116 | sensor.set_vflip(True) 117 | sensor.set_auto_gain(True) # do some calibration at the start 118 | sensor.set_auto_whitebal(True) 119 | sensor.skip_frames(60) # Let new settings take effect. 120 | sensor.set_auto_gain(False) # now turn off autocalibration before we start color tracking 121 | sensor.set_auto_whitebal(False) 122 | 123 | 124 | while(True): 125 | clock.tick() # Track elapsed milliseconds between snapshots(). 126 | switch = switch_pin.value() # get value, 0 or 1 127 | if switch == 1 or not RC_control: # Teensy says you're in MV mode 128 | img = sensor.snapshot().lens_corr(strength = 2.8, zoom = 1) 129 | img.binary([blue_threshold]) 130 | line = img.get_regression([binary_threshold],roi=rois, robust = False) # do linear regression for desired color 131 | if (line): 132 | img.draw_line(line.line(), color = 127) 133 | # print("Length", line.length(), "Magnitude", line.magnitude(), "Theta: ", line.theta(), "Rho: ", line.rho()) 134 | offset = line.line()[2] 135 | constrain(offset,0,320) 136 | offset = offset - 160 137 | offset_angle = math.acos(offset/320) 138 | offset_angle = math.degrees(offset_angle) - 90 139 | measured_angle = -math.atan2(line.line()[1]-line.line()[3],line.line()[0]-line.line()[2]) 140 | # print(line.line()) 141 | # Convert angle in radians to degrees. 142 | measured_angle = math.degrees(measured_angle) 143 | measured_angle = 90 - measured_angle # make straight ahead 0 144 | measured_angle = constrain(measured_angle, -90, 90) 145 | net_angle = 2*offset_angle - measured_angle 146 | # print("Offset Angle: ", offset_angle, "Measured Angle: ", measured_angle, "Net Angle: ", net_angle) 147 | 148 | # print("FPS %f" % clock.fps()) 149 | 150 | 151 | 152 | now = pyb.millis() 153 | if now > old_time + 0.02 : # time has passed since last measurement; do PID at 50Hz 154 | if now > led_time + 1000: # switch LED every second 155 | if led_state == True: 156 | led_control(0) # turn off LED 157 | led_state = False 158 | else: 159 | led_control(2) # turn on Green LED 160 | led_state = True 161 | led_time = now # reset time counter 162 | steer_angle = update_pid(net_angle) 163 | old_time = now 164 | steer (steer_angle) 165 | 166 | else: 167 | now = pyb.millis() 168 | if now > old_time + 1.0 : # time has passed since last measurement 169 | if now > led_time + 1000: # switch LED every second 170 | print("Under RC control") 171 | if led_state == True: 172 | led_control(0) # turn off LED 173 | led_state = False 174 | else: 175 | led_control(4) # turn on Blue LED 176 | led_state = True 177 | led_time = now # reset time counter 178 | 179 | -------------------------------------------------------------------------------- /MPVBlobs.py: -------------------------------------------------------------------------------- 1 | # Color Line Following Example with PID Steering 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import LED 10 | from pyb import Pin, Timer 11 | 12 | tim = Timer(4, freq=1000) # Frequency in Hz 13 | 14 | cruise_speed = 40 # how fast should the car drive, range from 0 to 100 15 | steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction 16 | steering_gain = 1.8 # calibration for your car's steering sensitivity 17 | steering_center = 40 # set to your car servo's center point 18 | kp = 0.8 # P term of the PID 19 | ki = 0.0 # I term of the PID 20 | kd = 0.6 # D term of the PID 21 | 22 | 23 | # Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max) 24 | # The below thresholds track in general red/green things. You may wish to tune them... 25 | # old thresholds = [(30, 100, 15, 127, 15, 127), # generic_red_thresholds 26 | # (30, 100, -64, -8, -32, 32), # generic_green_thresholds 27 | # (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds 28 | 29 | threshold_index = 0 30 | # 0 for red, 1 for green, 2 for blue 31 | 32 | thresholds = [( 0, 100, 17, 126, -28, 127), # generic_red_thresholds 33 | ( 0, 84, -128, -8, -23, 127), # generic_green_thresholds 34 | (0, 100, -128, -10, -128, 51)] # generic_blue_thresholds 35 | # You may pass up to 16 thresholds above. However, it's not really possible to segment any 36 | # scene with 16 thresholds before color thresholds start to overlap heavily. 37 | 38 | # Each roi is (x, y, w, h). The line detection algorithm will try to find the 39 | # centroid of the largest blob in each roi. The x position of the centroids 40 | # will then be averaged with different weights where the most weight is assigned 41 | # to the roi near the bottom of the image and less to the next roi and so on. 42 | ROIS = [ # [ROI, weight] 43 | (38,1,90,38, 0.0), 44 | (35,40,109,43,0.2), 45 | (0,79,160,41,1.0) 46 | ] 47 | 48 | blue_led = LED(3) 49 | 50 | 51 | old_error = 0 52 | measured_angle = 0 53 | set_angle = 90 # this is the desired steering angle (straight ahead) 54 | p_term = 0 55 | i_term = 0 56 | d_term = 0 57 | old_time = pyb.millis() 58 | 59 | 60 | 61 | def constrain(value, min, max): 62 | if value < min : 63 | return min 64 | if value > max : 65 | return max 66 | else: 67 | return value 68 | 69 | def steer(angle): 70 | global steering_gain, cruise_speed, steering_center 71 | angle = int(round((angle+steering_center)*steering_gain)) 72 | angle = constrain(angle, 0, 180) 73 | angle = 90 - angle 74 | left = (90 - angle) * (cruise_speed/100) 75 | left = constrain (left, 0, 100) 76 | right = (90 + angle) * (cruise_speed/100) 77 | right = constrain (right, 0, 100) 78 | print ("left: ", left) 79 | print ("right: ", right) 80 | # Generate a 1KHz square wave on TIM4 with each channel 81 | ch1 = tim.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width_percent=left) 82 | ch2 = tim.channel(2, Timer.PWM, pin=Pin("P8"), pulse_width_percent=right) 83 | 84 | def update_pid(): 85 | global old_time, old_error, measured_angle, set_angle 86 | global p_term, i_term, d_term 87 | now = pyb.millis() 88 | dt = now - old_time 89 | error = set_angle - measured_angle 90 | de = error - old_error 91 | 92 | p_term = kp * error 93 | i_term += ki * error 94 | i_term = constrain(i_term, 0, 100) 95 | d_term = (de / dt) * kd 96 | 97 | old_error = error 98 | output = steering_direction * (p_term + i_term + d_term) 99 | output = constrain(output, -50, 50) 100 | return output 101 | 102 | 103 | # Compute the weight divisor (we're computing this so you don't have to make weights add to 1). 104 | weight_sum = 0 105 | for r in ROIS: weight_sum += r[4] # r[4] is the roi weight. 106 | 107 | # Camera setup... 108 | clock = time.clock() # Tracks FPS. 109 | sensor.reset() # Initialize the camera sensor. 110 | sensor.__write_reg(0x6B, 0x22) # switches camera into advanced calibration mode. See this for more: http://forums.openmv.io/viewtopic.php?p=1358#p1358 111 | sensor.set_pixformat(sensor.RGB565) 112 | sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed. 113 | sensor.set_vflip(True) 114 | sensor.set_hmirror(True) 115 | sensor.set_auto_gain(True) # do some calibration at the start 116 | sensor.set_auto_whitebal(True) 117 | sensor.skip_frames(time = 2000) 118 | sensor.set_auto_gain(False) # now turn off autocalibration before we start color tracking 119 | sensor.set_auto_whitebal(False) 120 | 121 | 122 | while(True): 123 | clock.tick() # Track elapsed milliseconds between snapshots(). 124 | img = sensor.snapshot() # Take a picture and return the image. 125 | print("FPS: ",clock.fps()) 126 | centroid_sum = 0 127 | for r in ROIS: 128 | blobs = img.find_blobs([thresholds[threshold_index]], roi=r[0:4], merge=True) # r[0:4] is roi tuple. 129 | if blobs: 130 | # Find the index of the blob with the most pixels. 131 | most_pixels = 0 132 | largest_blob = 0 133 | for i in range(len(blobs)): 134 | if blobs[i].pixels() > most_pixels: 135 | most_pixels = blobs[i].pixels() 136 | largest_blob = i 137 | 138 | # Draw a rect around the blob. 139 | img.draw_rectangle(blobs[largest_blob].rect()) 140 | img.draw_cross(blobs[largest_blob].cx(), 141 | blobs[largest_blob].cy()) 142 | 143 | centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight. 144 | 145 | center_pos = (centroid_sum / weight_sum) # Determine center of line. 146 | 147 | # Convert the center_pos to a deflection angle. We're using a non-linear 148 | # operation so that the response gets stronger the farther off the line we 149 | # are. Non-linear operations are good to use on the output of algorithms 150 | # like this to cause a response "trigger". 151 | deflection_angle = 0 152 | # The 80 is from half the X res, the 60 is from half the Y res. The 153 | # equation below is just computing the angle of a triangle where the 154 | # opposite side of the triangle is the deviation of the center position 155 | # from the center and the adjacent side is half the Y res. This limits 156 | # the angle output to around -45 to 45. (It's not quite -45 and 45). 157 | deflection_angle = -math.atan((center_pos-80)/60) 158 | 159 | # Convert angle in radians to degrees. 160 | deflection_angle = math.degrees(deflection_angle) 161 | 162 | # Now you have an angle telling you how much to turn the robot by which 163 | # incorporates the part of the line nearest to the robot and parts of 164 | # the line farther away from the robot for a better prediction. 165 | # print("Turn Angle: %f" % deflection_angle) 166 | now = pyb.millis() 167 | if now > old_time + 0.02 : # time has passed since last measurement; do the PID at 50hz 168 | blue_led.on() 169 | measured_angle = deflection_angle + 90 170 | steer_angle = update_pid() 171 | old_time = now 172 | steer (steer_angle) 173 | # print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle)) 174 | blue_led.off() 175 | -------------------------------------------------------------------------------- /MVPRacer.py: -------------------------------------------------------------------------------- 1 | # Color Line Following Example with PID Steering 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import LED 10 | from pyb import Pin, Timer 11 | 12 | tim = Timer(4, freq=1000) # Frequency in Hz 13 | 14 | cruise_speed = 60 # how fast should the car drive, range from 0 to 100 15 | steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction 16 | steering_gain = 1.7 # calibration for your car's steering sensitivity 17 | steering_center = 60 # set to your car's steering center point 18 | kp = 0.8 # P term of the PID 19 | ki = 0.0 # I term of the PID 20 | kd = 0.4 # D term of the PID 21 | 22 | 23 | # Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max) 24 | # The below thresholds track in general red/green things. You may wish to tune them... 25 | # old thresholds = [(30, 100, 15, 127, 15, 127), # generic_red_thresholds 26 | # (30, 100, -64, -8, -32, 32), # generic_green_thresholds 27 | # (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds 28 | 29 | threshold_index = 2 30 | # 0 for red, 1 for green, 2 for blue 31 | 32 | thresholds = [(0, 100, 12, 127, 10, 127), # generic_red_thresholds 33 | (0, 100, -87, 18, -128, 33), # generic_green_thresholds 34 | (0, 100, -81, 127, -104, 10)] # generic_blue_thresholds 35 | # You may pass up to 16 thresholds above. However, it's not really possible to segment any 36 | # scene with 16 thresholds before color thresholds start to overlap heavily. 37 | 38 | # Each roi is (x, y, w, h). The line detection algorithm will try to find the 39 | # centroid of the largest blob in each roi. The x position of the centroids 40 | # will then be averaged with different weights where the most weight is assigned 41 | # to the roi near the bottom of the image and less to the next roi and so on. 42 | ROIS = [ # [ROI, weight] 43 | (38,1,90,38, 0.4), 44 | (35,40,109,43,0.2), 45 | (0,79,160,41,0.6) 46 | ] 47 | 48 | blue_led = LED(3) 49 | 50 | 51 | old_error = 0 52 | measured_angle = 0 53 | set_angle = 90 # this is the desired steering angle (straight ahead) 54 | p_term = 0 55 | i_term = 0 56 | d_term = 0 57 | old_time = pyb.millis() 58 | radians_degrees = 57.3 # constant to convert from radians to degrees 59 | 60 | 61 | 62 | def constrain(value, min, max): 63 | if value < min : 64 | return min 65 | if value > max : 66 | return max 67 | else: 68 | return value 69 | 70 | ch1 = tim.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width_percent=0) 71 | ch2 = tim.channel(2, Timer.PWM, pin=Pin("P8"), pulse_width_percent=0) 72 | 73 | def steer(angle): 74 | global steering_gain, cruise_speed, steering_center 75 | angle = int(round((angle+steering_center)*steering_gain)) 76 | angle = constrain(angle, 0, 180) 77 | angle = 90 - angle 78 | angle = radians_degrees * math.tan(angle/radians_degrees) # take the tangent to create a non-linear response curver 79 | left = (90 - angle) * (cruise_speed/100) 80 | left = constrain (left, 0, 100) 81 | right = (90 + angle) * (cruise_speed/100) 82 | right = constrain (right, 0, 100) 83 | print ("left: ", left) 84 | print ("right: ", right) 85 | # Generate a 1KHz square wave on TIM4 with each channel 86 | ch1.pulse_width_percent(left) 87 | ch2.pulse_width_percent(right) 88 | 89 | def update_pid(): 90 | global old_time, old_error, measured_angle, set_angle 91 | global p_term, i_term, d_term 92 | now = pyb.millis() 93 | dt = now - old_time 94 | error = set_angle - measured_angle 95 | de = error - old_error 96 | 97 | p_term = kp * error 98 | i_term += ki * error 99 | i_term = constrain(i_term, 0, 100) 100 | d_term = (de / dt) * kd 101 | 102 | old_error = error 103 | output = steering_direction * (p_term + i_term + d_term) 104 | output = constrain(output, -50, 50) 105 | return output 106 | 107 | 108 | # Compute the weight divisor (we're computing this so you don't have to make weights add to 1). 109 | weight_sum = 0 110 | for r in ROIS: weight_sum += r[4] # r[4] is the roi weight. 111 | 112 | # Camera setup... 113 | clock = time.clock() # Tracks FPS. 114 | sensor.reset() # Initialize the camera sensor. 115 | sensor.__write_reg(0x6B, 0x22) # switches camera into advanced calibration mode. See this for more: http://forums.openmv.io/viewtopic.php?p=1358#p1358 116 | sensor.set_pixformat(sensor.RGB565) 117 | sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed. 118 | sensor.set_vflip(True) 119 | sensor.set_hmirror(True) 120 | sensor.set_auto_gain(True) # do some calibration at the start 121 | sensor.set_auto_whitebal(True) 122 | sensor.skip_frames(time = 0) # When you're inside, set time to 2000 to do a white balance calibration. Outside, this can be 0 123 | sensor.set_auto_gain(False) # now turn off autocalibration before we start color tracking 124 | sensor.set_auto_whitebal(False) 125 | 126 | 127 | while(True): 128 | clock.tick() # Track elapsed milliseconds between snapshots(). 129 | img = sensor.snapshot().histeq() # Take a picture and return the image. The "histeq()" function does a histogram equalization to compensate for lighting changes 130 | print("FPS: ",clock.fps()) 131 | centroid_sum = 0 132 | for r in ROIS: 133 | blobs = img.find_blobs([thresholds[threshold_index]], roi=r[0:4], merge=True) # r[0:4] is roi tuple. 134 | if blobs: 135 | # Find the index of the blob with the most pixels. 136 | most_pixels = 0 137 | largest_blob = 0 138 | for i in range(len(blobs)): 139 | if blobs[i].pixels() > most_pixels: 140 | most_pixels = blobs[i].pixels() 141 | largest_blob = i 142 | 143 | # Draw a rect around the blob. 144 | img.draw_rectangle(blobs[largest_blob].rect()) 145 | img.draw_cross(blobs[largest_blob].cx(), 146 | blobs[largest_blob].cy()) 147 | 148 | centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight. 149 | 150 | center_pos = (centroid_sum / weight_sum) # Determine center of line. 151 | 152 | # Convert the center_pos to a deflection angle. We're using a non-linear 153 | # operation so that the response gets stronger the farther off the line we 154 | # are. Non-linear operations are good to use on the output of algorithms 155 | # like this to cause a response "trigger". 156 | deflection_angle = 0 157 | # The 80 is from half the X res, the 60 is from half the Y res. The 158 | # equation below is just computing the angle of a triangle where the 159 | # opposite side of the triangle is the deviation of the center position 160 | # from the center and the adjacent side is half the Y res. This limits 161 | # the angle output to around -45 to 45. (It's not quite -45 and 45). 162 | deflection_angle = -math.atan((center_pos-80)/60) 163 | 164 | # Convert angle in radians to degrees. 165 | deflection_angle = math.degrees(deflection_angle) 166 | 167 | # Now you have an angle telling you how much to turn the robot by which 168 | # incorporates the part of the line nearest to the robot and parts of 169 | # the line farther away from the robot for a better prediction. 170 | # print("Turn Angle: %f" % deflection_angle) 171 | now = pyb.millis() 172 | if now > old_time + 0.02 : # time has passed since last measurement; do the PID at 50hz 173 | blue_led.on() 174 | measured_angle = deflection_angle + 90 175 | steer_angle = update_pid() 176 | old_time = now 177 | steer (steer_angle) 178 | # print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle)) 179 | blue_led.off() 180 | -------------------------------------------------------------------------------- /MVPRacerNewShield.py: -------------------------------------------------------------------------------- 1 | # Color Line Following Example with PID Steering 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import LED 10 | from pyb import Pin, Timer 11 | 12 | tim = Timer(4, freq=1000) # Frequency in Hz 13 | 14 | cruise_speed = 60 # how fast should the car drive, range from 0 to 100 15 | steering_direction = 1 # use this to revers the steering if your car goes in the wrong direction 16 | steering_gain = 1.7 # calibration for your car's steering sensitivity 17 | steering_center = 85 # set to your car's center point 18 | kp = 0.8 # P term of the PID 19 | ki = 0.0 # I term of the PID 20 | kd = 0.4 # D term of the PID 21 | 22 | Left1 = Pin('P0', Pin.OUT_PP) 23 | Left2 = Pin('P1', Pin.OUT_PP) 24 | Right1 = Pin('P2', Pin.OUT_PP) 25 | Right2 = Pin('P3', Pin.OUT_PP) 26 | null = Left1.value(False) 27 | null = Left2.value(True) 28 | null = Right1.value(True) 29 | null = Right2.value(False) 30 | 31 | 32 | # Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max) 33 | # The below thresholds track in general red/green things. You may wish to tune them... 34 | # old thresholds = [(30, 100, 15, 127, 15, 127), # generic_red_thresholds 35 | # (30, 100, -64, -8, -32, 32), # generic_green_thresholds 36 | # (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds 37 | 38 | threshold_index = 0 39 | # 0 for red, 1 for green, 2 for blue 40 | 41 | thresholds = [( 0, 100, 31, 127, -25, 127), # generic_red_thresholds 42 | (0, 100, -87, 18, -128, 33), # generic_green_thresholds 43 | (0, 100, -128, -10, -128, 51)] # generic_blue_thresholds 44 | # You may pass up to 16 thresholds above. However, it's not really possible to segment any 45 | # scene with 16 thresholds before color thresholds start to overlap heavily. 46 | 47 | # Each roi is (x, y, w, h). The line detection algorithm will try to find the 48 | # centroid of the largest blob in each roi. The x position of the centroids 49 | # will then be averaged with different weights where the most weight is assigned 50 | # to the roi near the bottom of the image and less to the next roi and so on. 51 | ROIS = [ # [ROI, weight] 52 | (38,1,90,38, 0.4), 53 | (35,40,109,43,0.2), 54 | (0,79,160,41,0.6) 55 | ] 56 | 57 | blue_led = LED(3) 58 | 59 | 60 | old_error = 0 61 | measured_angle = 0 62 | set_angle = 90 # this is the desired steering angle (straight ahead) 63 | p_term = 0 64 | i_term = 0 65 | d_term = 0 66 | old_time = pyb.millis() 67 | radians_degrees = 57.3 # constant to convert from radians to degrees 68 | 69 | 70 | def constrain(value, min, max): 71 | if value < min : 72 | return min 73 | if value > max : 74 | return max 75 | else: 76 | return value 77 | 78 | ch1 = tim.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width_percent=0) 79 | ch2 = tim.channel(2, Timer.PWM, pin=Pin("P8"), pulse_width_percent=0) 80 | 81 | def steer(angle): 82 | global steering_gain, cruise_speed, steering_center 83 | angle = int(round(steering_direction*(angle+steering_center)*steering_gain)) 84 | angle = constrain(angle, 0, 180) 85 | angle = 90 - angle 86 | angle = radians_degrees * math.tan(angle/radians_degrees) # take the tangent to create a non-linear response curver 87 | left = (90 - angle) * (cruise_speed/100) 88 | left = constrain (left, 0, 100) 89 | right = (90 + angle) * (cruise_speed/100) 90 | right = constrain (right, 0, 100) 91 | print ("left: ", left) 92 | print ("right: ", right) 93 | # Generate a 1KHz square wave on TIM4 with each channel 94 | ch1.pulse_width_percent(left) 95 | ch2.pulse_width_percent(right) 96 | 97 | def update_pid(): 98 | global old_time, old_error, measured_angle, set_angle 99 | global p_term, i_term, d_term 100 | now = pyb.millis() 101 | dt = now - old_time 102 | error = set_angle - measured_angle 103 | de = error - old_error 104 | 105 | p_term = kp * error 106 | i_term += ki * error 107 | i_term = constrain(i_term, 0, 100) 108 | d_term = (de / dt) * kd 109 | 110 | old_error = error 111 | output = steering_direction * (p_term + i_term + d_term) 112 | output = constrain(output, -50, 50) 113 | return output 114 | 115 | 116 | # Compute the weight divisor (we're computing this so you don't have to make weights add to 1). 117 | weight_sum = 0 118 | for r in ROIS: weight_sum += r[4] # r[4] is the roi weight. 119 | 120 | # Camera setup... 121 | clock = time.clock() # Tracks FPS. 122 | sensor.reset() # Initialize the camera sensor. 123 | sensor.__write_reg(0x6B, 0x22) # switches camera into advanced calibration mode. See this for more: http://forums.openmv.io/viewtopic.php?p=1358#p1358 124 | sensor.set_pixformat(sensor.RGB565) 125 | sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed. 126 | sensor.set_vflip(True) 127 | sensor.set_hmirror(True) 128 | sensor.set_auto_gain(True) # do some calibration at the start 129 | sensor.set_auto_whitebal(True) 130 | sensor.skip_frames(time = 0) # When you're inside, set time to 2000 to do a white balance calibration. Outside, this can be 0 131 | sensor.set_auto_gain(False) # now turn off autocalibration before we start color tracking 132 | sensor.set_auto_whitebal(False) 133 | 134 | 135 | while(True): 136 | clock.tick() # Track elapsed milliseconds between snapshots(). 137 | img = sensor.snapshot().histeq() # Take a picture and return the image. The "histeq()" function does a histogram equalization to compensate for lighting changes 138 | print("FPS: ",clock.fps()) 139 | centroid_sum = 0 140 | for r in ROIS: 141 | blobs = img.find_blobs([thresholds[threshold_index]], roi=r[0:4], merge=True) # r[0:4] is roi tuple. 142 | if blobs: 143 | # Find the index of the blob with the most pixels. 144 | most_pixels = 0 145 | largest_blob = 0 146 | for i in range(len(blobs)): 147 | if blobs[i].pixels() > most_pixels: 148 | most_pixels = blobs[i].pixels() 149 | largest_blob = i 150 | 151 | # Draw a rect around the blob. 152 | img.draw_rectangle(blobs[largest_blob].rect()) 153 | img.draw_cross(blobs[largest_blob].cx(), 154 | blobs[largest_blob].cy()) 155 | 156 | centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight. 157 | 158 | center_pos = (centroid_sum / weight_sum) # Determine center of line. 159 | 160 | # Convert the center_pos to a deflection angle. We're using a non-linear 161 | # operation so that the response gets stronger the farther off the line we 162 | # are. Non-linear operations are good to use on the output of algorithms 163 | # like this to cause a response "trigger". 164 | deflection_angle = 0 165 | # The 80 is from half the X res, the 60 is from half the Y res. The 166 | # equation below is just computing the angle of a triangle where the 167 | # opposite side of the triangle is the deviation of the center position 168 | # from the center and the adjacent side is half the Y res. This limits 169 | # the angle output to around -45 to 45. (It's not quite -45 and 45). 170 | deflection_angle = -math.atan((center_pos-80)/60) 171 | 172 | # Convert angle in radians to degrees. 173 | deflection_angle = math.degrees(deflection_angle) 174 | 175 | # Now you have an angle telling you how much to turn the robot by which 176 | # incorporates the part of the line nearest to the robot and parts of 177 | # the line farther away from the robot for a better prediction. 178 | # print("Turn Angle: %f" % deflection_angle) 179 | now = pyb.millis() 180 | if now > old_time + 0.02 : # time has passed since last measurement; do the PID at 50hz 181 | blue_led.on() 182 | measured_angle = deflection_angle + 90 183 | steer_angle = update_pid() 184 | old_time = now 185 | steer (steer_angle) 186 | # print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle)) 187 | blue_led.off() 188 | -------------------------------------------------------------------------------- /MVR.py: -------------------------------------------------------------------------------- 1 | # Color Line Following Example with PID Steering 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import LED 10 | from pyb import Pin, Timer 11 | 12 | # these are motor driver pins, which set the direction of each motor 13 | pinADir0 = pyb.Pin('P0', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE) 14 | pinADir1 = pyb.Pin('P1', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE) 15 | pinBDir0 = pyb.Pin('P2', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE) 16 | pinBDir1 = pyb.Pin('P3', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE) 17 | 18 | 19 | # Dir0/1 must be not equal to each other for forward or backwards 20 | # operation. If they are equal then that's a brake operation. 21 | # If they are not equal then the motor will spin one way other the 22 | # other depending on its hookup and the value of channel 0. 23 | 24 | pinADir0.value(1) 25 | pinADir1.value(0) 26 | 27 | 28 | # Dir0/1 must be not equal to each other for forward or backwards 29 | # operation. If they are equal then that's a brake operation. 30 | # If they are not equal then the motor will spin one way other the 31 | # other depending on its hookup and the value of channel 0. 32 | 33 | pinBDir0.value(0) 34 | pinBDir1.value(1) 35 | 36 | tim = Timer(4, freq=1000) # Frequency in Hz 37 | 38 | cruise_speed = 60 # how fast should the car drive, range from 0 to 100 39 | steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction 40 | steering_gain = 1.7 # calibration for your car's steering sensitivity 41 | steering_center = 30 # set to your car servo's center point 42 | kp = 0.8 # P term of the PID 43 | ki = 0.0 # I term of the PID 44 | kd = 0.4 # D term of the PID 45 | 46 | 47 | # Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max) 48 | # The below thresholds track in general red/green things. You may wish to tune them... 49 | # old thresholds = [(30, 100, 15, 127, 15, 127), # generic_red_thresholds 50 | # (30, 100, -64, -8, -32, 32), # generic_green_thresholds 51 | # (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds 52 | 53 | threshold_index = 0 54 | # 0 for red, 1 for green, 2 for blue 55 | 56 | thresholds = [( 0, 100, 36, 127, -25, 127), # generic_red_thresholds 57 | (0, 100, -87, 18, -128, 33), # generic_green_thresholds 58 | (0, 100, -128, -10, -128, 51)] # generic_blue_thresholds 59 | # You may pass up to 16 thresholds above. However, it's not really possible to segment any 60 | # scene with 16 thresholds before color thresholds start to overlap heavily. 61 | 62 | # Each roi is (x, y, w, h). The line detection algorithm will try to find the 63 | # centroid of the largest blob in each roi. The x position of the centroids 64 | # will then be averaged with different weights where the most weight is assigned 65 | # to the roi near the bottom of the image and less to the next roi and so on. 66 | ROIS = [ # [ROI, weight] 67 | (38,1,90,38, 0.4), 68 | (35,40,109,43,0.6), 69 | (0,79,160,41,0.2) 70 | ] 71 | 72 | blue_led = LED(3) 73 | 74 | 75 | 76 | 77 | old_error = 0 78 | measured_angle = 0 79 | set_angle = 90 # this is the desired steering angle (straight ahead) 80 | p_term = 0 81 | i_term = 0 82 | d_term = 0 83 | old_time = pyb.millis() 84 | radians_degrees = 57.3 # constant to convert from radians to degrees 85 | 86 | 87 | 88 | def constrain(value, min, max): 89 | if value < min : 90 | return min 91 | if value > max : 92 | return max 93 | else: 94 | return value 95 | 96 | ch1 = tim.channel(1, pyb.Timer.PWM, pin=pyb.Pin("P7")) 97 | ch2 = tim.channel(2, pyb.Timer.PWM, pin=pyb.Pin("P8")) 98 | 99 | def steer(angle): 100 | global steering_gain, cruise_speed, steering_center 101 | angle = int(round((angle+steering_center)*steering_gain)) 102 | angle = constrain(angle, 0, 180) 103 | angle = 90 - angle 104 | angle = radians_degrees * math.tan(angle/radians_degrees) # take the tangent to create a non-linear response curver 105 | left = (90 - angle) * (cruise_speed/100) 106 | left = constrain (left, 0, 100) 107 | right = (90 + angle) * (cruise_speed/100) 108 | right = constrain (right, 0, 100) 109 | print ("left: ", left) 110 | print ("right: ", right) 111 | # Generate a 1KHz square wave on TIM4 with each channel 112 | ch1.pulse_width_percent(left) 113 | ch2.pulse_width_percent(right) 114 | 115 | def update_pid(): 116 | global old_time, old_error, measured_angle, set_angle 117 | global p_term, i_term, d_term 118 | now = pyb.millis() 119 | dt = now - old_time 120 | error = set_angle - measured_angle 121 | de = error - old_error 122 | 123 | p_term = kp * error 124 | i_term += ki * error 125 | i_term = constrain(i_term, 0, 100) 126 | d_term = (de / dt) * kd 127 | 128 | old_error = error 129 | output = steering_direction * (p_term + i_term + d_term) 130 | output = constrain(output, -50, 50) 131 | return output 132 | 133 | 134 | # Compute the weight divisor (we're computing this so you don't have to make weights add to 1). 135 | weight_sum = 0 136 | for r in ROIS: weight_sum += r[4] # r[4] is the roi weight. 137 | 138 | # Camera setup... 139 | clock = time.clock() # Tracks FPS. 140 | sensor.reset() # Initialize the camera sensor. 141 | sensor.__write_reg(0x6B, 0x22) # switches camera into advanced calibration mode. See this for more: http://forums.openmv.io/viewtopic.php?p=1358#p1358 142 | sensor.set_pixformat(sensor.RGB565) 143 | sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed. 144 | sensor.set_vflip(True) 145 | sensor.set_hmirror(True) 146 | sensor.set_auto_gain(True) # do some calibration at the start 147 | sensor.set_auto_whitebal(True) 148 | sensor.skip_frames(time = 0) # When you're inside, set time to 2000 to do a white balance calibration. Outside, this can be 0 149 | sensor.set_auto_gain(False) # now turn off autocalibration before we start color tracking 150 | sensor.set_auto_whitebal(False) 151 | 152 | 153 | while(True): 154 | clock.tick() # Track elapsed milliseconds between snapshots(). 155 | img = sensor.snapshot().histeq() # Take a picture and return the image. The "histeq()" function does a histogram equalization to compensate for lighting changes 156 | print("FPS: ",clock.fps()) 157 | centroid_sum = 0 158 | for r in ROIS: 159 | blobs = img.find_blobs([thresholds[threshold_index]], roi=r[0:4], merge=True) # r[0:4] is roi tuple. 160 | if blobs: 161 | # Find the index of the blob with the most pixels. 162 | most_pixels = 0 163 | largest_blob = 0 164 | for i in range(len(blobs)): 165 | if blobs[i].pixels() > most_pixels: 166 | most_pixels = blobs[i].pixels() 167 | largest_blob = i 168 | 169 | # Draw a rect around the blob. 170 | img.draw_rectangle(blobs[largest_blob].rect()) 171 | img.draw_cross(blobs[largest_blob].cx(), 172 | blobs[largest_blob].cy()) 173 | 174 | centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight. 175 | 176 | center_pos = (centroid_sum / weight_sum) # Determine center of line. 177 | 178 | # Convert the center_pos to a deflection angle. We're using a non-linear 179 | # operation so that the response gets stronger the farther off the line we 180 | # are. Non-linear operations are good to use on the output of algorithms 181 | # like this to cause a response "trigger". 182 | deflection_angle = 0 183 | # The 80 is from half the X res, the 60 is from half the Y res. The 184 | # equation below is just computing the angle of a triangle where the 185 | # opposite side of the triangle is the deviation of the center position 186 | # from the center and the adjacent side is half the Y res. This limits 187 | # the angle output to around -45 to 45. (It's not quite -45 and 45). 188 | deflection_angle = -math.atan((center_pos-80)/60) 189 | 190 | # Convert angle in radians to degrees. 191 | deflection_angle = math.degrees(deflection_angle) 192 | 193 | # Now you have an angle telling you how much to turn the robot by which 194 | # incorporates the part of the line nearest to the robot and parts of 195 | # the line farther away from the robot for a better prediction. 196 | # print("Turn Angle: %f" % deflection_angle) 197 | now = pyb.millis() 198 | if now > old_time + 0.02 : # time has passed since last measurement; do the PID at 50hz 199 | blue_led.on() 200 | measured_angle = deflection_angle + 90 201 | steer_angle = update_pid() 202 | old_time = now 203 | steer (steer_angle) 204 | # print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle)) 205 | blue_led.off() 206 | -------------------------------------------------------------------------------- /OpenMVArduino/OpenMVArduino.ino: -------------------------------------------------------------------------------- 1 | /* Miminimum sketch to translate incoming serial from an OpenMV board to Ardunio commands for motor and servo 2 | * Written by Chris Anderson, DIYRobocars, 2017 3 | */ 4 | 5 | #include //servo library 6 | #include 7 | 8 | SoftwareSerial mySerial(2, 5); // RX, TX 9 | Servo myservoa, myservob; // create servo objects to control servos 10 | int SteeringPin = 10; // This corresponds to Servo Output 1 11 | int MotorPin = 6; // This correspondes to Servo Output 2 12 | int CH3 = 5; // Corresponds to Servo Output 3 13 | int CH4 = 4; // Corresponds to Servo Output 4 14 | int CH5 = 9; // Corresponds to Servo Outut 5 15 | int steer, motor; 16 | 17 | unsigned long time; 18 | unsigned long lasttime = 0; 19 | bool LEDState = LOW; 20 | 21 | const int BAUD_RATE = 9600; 22 | 23 | void setup() { 24 | Serial.begin(BAUD_RATE); 25 | myservoa.attach(SteeringPin);// attach servo on Output 1 to servo object 26 | myservob.attach(MotorPin);// attach servo on Output 2 to servo object 27 | mySerial.begin(9600); // This is for the debug console 28 | mySerial.println("Hello, world"); 29 | pinMode(LED_BUILTIN, OUTPUT); // enable LED 30 | } 31 | 32 | void loop() { 33 | time = millis(); 34 | if (time > lasttime + 1000) { // flash the LED every second to show "resting" mode 35 | lasttime = time; 36 | LEDState = !LEDState; // reverse the LED state 37 | digitalWrite(LED_BUILTIN, LEDState); // turn on or off the LED 38 | } 39 | while (Serial.available() > 0) { 40 | // look for the next valid integer in the incoming serial stream: 41 | int tempsteer = Serial.parseInt(); 42 | // mySerial.println(steer); // turn this on for debgging. Warning: it will make the servos pulse 43 | // do it again: 44 | int tempmotor = Serial.parseInt(); 45 | // mySerial.println(motor); // turn this on for debgging. Warning: it will make the servos pulse 46 | // look for the newline. That's the end of the commands 47 | if (Serial.read() == '\n') { 48 | steer=tempsteer; 49 | motor=tempmotor; 50 | LEDState = !LEDState; // reverse the LED state 51 | digitalWrite(LED_BUILTIN, LEDState); // turn on or off the LED to show activity 52 | } 53 | } 54 | myservoa.write(steer); // send values to output 55 | myservob.write(motor); 56 | delay (20); 57 | } 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /OpenMVTeensy/OpenMVTeensy.ino: -------------------------------------------------------------------------------- 1 | #include //servo library 2 | 3 | Servo myservoa, myservob; // create servo objects to control servos 4 | 5 | #define MotorPin 20 6 | #define SteeringPin 21 7 | #define RC1_pin 3 8 | #define RC2_pin 4 9 | #define RC3_pin 5 10 | #define SwitchPin 10 11 | 12 | const int BAUD_RATE = 19200; 13 | int RC; 14 | unsigned long RC1_value; 15 | unsigned long RC2_value; 16 | unsigned long RC3_value; 17 | boolean RC_throttle = true; // if you want to control the throttle manually when it's under OpenMV control 18 | 19 | int steer, motor; 20 | 21 | unsigned long time; 22 | unsigned long lasttime = 0; 23 | bool LEDState = LOW; 24 | 25 | void setup() { 26 | Serial.begin(BAUD_RATE); // USB port 27 | Serial1.begin(BAUD_RATE); //OpenMV connection 28 | pinMode(RC1_pin, INPUT); 29 | pinMode(RC2_pin, INPUT); 30 | pinMode(RC3_pin, INPUT); 31 | pinMode(SwitchPin, OUTPUT); 32 | myservoa.attach(SteeringPin);// attach servo on Output 1 to servo object 33 | myservob.attach(MotorPin);// attach servo on Output 2 to servo object 34 | pinMode(LED_BUILTIN, OUTPUT); // enable LED 35 | } 36 | 37 | void RCcontrol() { 38 | Serial.println(RC3_value); // print switch state for debugging 39 | digitalWrite(SwitchPin, LOW); 40 | RC1_value = pulseIn(RC1_pin, HIGH); // read rc inputs 41 | RC2_value = pulseIn(RC2_pin, HIGH); 42 | myservoa.write(RC1_value); // mirror values to output 43 | myservob.write(RC2_value); 44 | delay (20); 45 | 46 | } 47 | 48 | void OpenMVcontrol() { 49 | digitalWrite(SwitchPin, HIGH); // tell OpenMV to take control and start sending data 50 | while (Serial1.available() > 0) { 51 | // look for the next valid integer in the incoming serial stream: 52 | int tempsteer = Serial1.parseInt(); 53 | // do it again for motor: 54 | int tempmotor = Serial1.parseInt(); 55 | // look for the newline. That's the end of the commands 56 | if (Serial1.read() == '\n') { 57 | steer=tempsteer; 58 | motor=tempmotor; 59 | if (RC_throttle) { 60 | motor = pulseIn(RC2_pin, HIGH); 61 | } 62 | LEDState = !LEDState; // reverse the LED state 63 | digitalWrite(LED_BUILTIN, LEDState); // turn on or off the LED to show activity 64 | steer = constrain(steer,1200,1700); 65 | motor = constrain(motor,1000,2000); 66 | Serial.print("Steer: "); 67 | Serial.print(tempsteer); 68 | Serial.print(" Motor: "); 69 | Serial.println(tempmotor); 70 | myservoa.write(steer); // send values to output 71 | myservob.write(motor); 72 | } 73 | } 74 | } 75 | 76 | void loop() { 77 | time = millis(); 78 | if (time > lasttime + 1000) { // flash the LED every second to show "resting" mode 79 | lasttime = time; 80 | LEDState = !LEDState; // reverse the LED state 81 | digitalWrite(LED_BUILTIN, LEDState); // turn on or off the LED 82 | } 83 | RC3_value = pulseIn(RC3_pin, HIGH); 84 | if (RC3_value > 1500) {RCcontrol();} // Use the CH5 switch to decide whether to pass through RC commands or take OpenMV commands 85 | else {OpenMVcontrol();} 86 | 87 | } 88 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OpenMVrover 2 | Autonomous car using the OpenMV camera 3 | -------------------------------------------------------------------------------- /Rover2020.py: -------------------------------------------------------------------------------- 1 | # This file is part of the OpenMV project. 2 | # Copyright (c) 2013-2017 Ibrahim Abdelkader & Kwabena W. Agyeman 3 | # Support for OpenMV Motor Shield by Chris Anderson, DIY Robocars 4 | # This work is licensed under the MIT license, see the file LICENSE for details. 5 | 6 | 7 | import sensor, pyb, image, math, time 8 | from pyb import LED 9 | from pyb import UART 10 | from pyb import Pin 11 | 12 | 13 | 14 | 15 | ########### 16 | # Settings 17 | ########### 18 | 19 | 20 | 21 | cruise_speed = 1575 # how fast should the car drive, range from 1000 (full backwards) to 2000 (full forwards). 1500 is stopped. 1575 is slowest it can go 22 | steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction 23 | steering_gain = 2.5 # calibration for your car's steering sensitivity 24 | steering_center = 0 # set to your car servo's center point 25 | 26 | 27 | RC_control = True # Set to false if you're just benchtop testing 28 | 29 | switch_pin = Pin('P7', Pin.IN, Pin.PULL_DOWN) 30 | 31 | 32 | COLOR_THRESHOLDS = [(0, 100, -128, 127, -128, -35)] # lights 33 | #COLOR_THRESHOLDS = (37, 100, -128, 8, -128, -29) # sunlight 34 | 35 | TARGET_POINTS = [(70, 30), # (x, y) CHANGE ME! 36 | (250, 30), # (x, y) CHANGE ME! 37 | (320, 179), # (x, y) CHANGE ME! 38 | (0, 179)] # (x, y) CHANGE ME! 39 | 40 | 41 | 42 | COLOR_LINE_FOLLOWING = True # False to use grayscale thresholds, true to use color thresholds. 43 | GRAYSCALE_THRESHOLDS = [(240, 255)] # White Line. 44 | COLOR_HIGH_LIGHT_THRESHOLDS = [(80, 100, -10, 10, -10, 10)] 45 | GRAYSCALE_HIGH_LIGHT_THRESHOLDS = [(250, 255)] 46 | BINARY_VIEW = False # Helps debugging but costs FPS if on. 47 | FRAME_SIZE = sensor.QVGA # Frame size. 48 | FRAME_REGION = 0.75 # Percentage of the image from the bottom (0 - 1.0). 49 | FRAME_WIDE = 1.0 # Percentage of the frame width. 50 | 51 | AREA_THRESHOLD = 0 # Raise to filter out false detections. 52 | PIXELS_THRESHOLD = 40 # Raise to filter out false detections. 53 | MAG_THRESHOLD = 4 # Raise to filter out false detections. 54 | MIXING_RATE = 0.9 # Percentage of a new line detection to mix into current steering. 55 | 56 | # Tweak these values for your robocar. 57 | THROTTLE_CUT_OFF_ANGLE = 1.0 # Maximum angular distance from 90 before we cut speed [0.0-90.0). 58 | THROTTLE_CUT_OFF_RATE = 0.5 # How much to cut our speed boost (below) once the above is passed (0.0-1.0]. 59 | THROTTLE_GAIN = 0.0 # e.g. how much to speed up on a straight away 60 | THROTTLE_OFFSET = 30.0 # e.g. default speed (0 to 100) 61 | THROTTLE_P_GAIN = 1.0 62 | THROTTLE_I_GAIN = 0.0 63 | THROTTLE_I_MIN = -0.0 64 | THROTTLE_I_MAX = 0.0 65 | THROTTLE_D_GAIN = 0.0 66 | 67 | # Tweak these values for your robocar if you're using servos. 68 | THROTTLE_SERVO_MIN_US = 1500 69 | THROTTLE_SERVO_MAX_US = 2000 70 | 71 | # Tweak these values for your robocar. 72 | STEERING_SERVO_MIN_US = 700 73 | STEERING_SERVO_MAX_US = 2300 74 | 75 | # Tweak these values for your robocar. 76 | STEERING_OFFSET = 90 # Change this if you need to fix an imbalance in your car (0 to 180). 77 | STEERING_P_GAIN = -15.0 # Make this smaller as you increase your speed and vice versa. 78 | STEERING_I_GAIN = 0.0 79 | STEERING_I_MIN = -0.0 80 | STEERING_I_MAX = 0.0 81 | STEERING_D_GAIN = -12 # Make this larger as you increase your speed and vice versa. 82 | 83 | 84 | 85 | ########### 86 | # Setup 87 | ########### 88 | 89 | FRAME_REGION = max(min(FRAME_REGION, 1.0), 0.0) 90 | FRAME_WIDE = max(min(FRAME_WIDE, 1.0), 0.0) 91 | MIXING_RATE = max(min(MIXING_RATE, 1.0), 0.0) 92 | 93 | THROTTLE_CUT_OFF_ANGLE = max(min(THROTTLE_CUT_OFF_ANGLE, 89.99), 0) 94 | THROTTLE_CUT_OFF_RATE = max(min(THROTTLE_CUT_OFF_RATE, 1.0), 0.01) 95 | 96 | THROTTLE_OFFSET = max(min(THROTTLE_OFFSET, 100), 0) 97 | STEERING_OFFSET = max(min(STEERING_OFFSET, 180), 0) 98 | 99 | 100 | 101 | 102 | uart = UART(3, 19200) # Use UART 3 (pins 4 and 5) 103 | 104 | 105 | red_led = LED(1) 106 | green_led = LED(2) 107 | blue_led = LED(3) 108 | ir_led = LED(4) 109 | led_state = True 110 | led_time = pyb.millis() 111 | 112 | 113 | 114 | def led_control(x): 115 | if (x&1)==0: red_led.off() 116 | elif (x&1)==1: red_led.on() 117 | if (x&2)==0: green_led.off() 118 | elif (x&2)==2: green_led.on() 119 | if (x&4)==0: blue_led.off() 120 | elif (x&4)==4: blue_led.on() 121 | if (x&8)==0: ir_led.off() 122 | elif (x&8)==8: ir_led.on() 123 | 124 | 125 | def constrain(value, min, max): 126 | if value < min : 127 | return min 128 | if value > max : 129 | return max 130 | else: 131 | return value 132 | 133 | #def steer(throttle, angle): 134 | # global steering_gain, cruise_speed, steering_center 135 | # angle = int(round(angle+steering_center)) 136 | # angle = constrain(angle, 0, 180) 137 | # angle = angle - 90 138 | # angle = radians_degrees * math.tan(angle/radians_degrees) # take the tangent to create a non-linear response curver 139 | # angle = angle * steering_gain 140 | # print ("Calculated angle", angle) 141 | 142 | 143 | 144 | # This function maps the output of the linear regression function to a driving vector for steering 145 | # the robocar. See https://openmv.io/blogs/news/linear-regression-line-following for more info. 146 | 147 | old_cx_normal = None 148 | def figure_out_my_steering(line, img): 149 | global old_cx_normal 150 | 151 | # Rho is computed using the inverse of this code below in the actual OpenMV Cam code. 152 | # This formula comes from the Hough line detection formula (see the wikipedia page for more). 153 | # Anyway, the output of this calculations below are a point centered vertically in the middle 154 | # of the image and to the left or right such that the line goes through it (cx may be off the image). 155 | cy = img.height() / 2 156 | cx = (line.rho() - (cy * math.sin(math.radians(line.theta())))) / math.cos(math.radians(line.theta())) 157 | 158 | # "cx_middle" is now the distance from the center of the line. This is our error method to stay 159 | # on the line. "cx_normal" normalizes the error to something like -1/+1 (it will go over this). 160 | cx_middle = cx - (img.width() / 2) 161 | cx_normal = cx_middle / (img.width() / 2) 162 | # Note that "cx_normal" may be larger than -1/+1. When the value is between -1/+1 this means the 163 | # robot is driving basically straight and needs to only turn lightly left or right. When the value 164 | # is outside -1/+1 it means you need to turn VERY hard to the left or right to get back on the 165 | # line. This maps to the case of the robot driving into a horizontal line. "cx_normal" will 166 | # then approach -inf/+inf depending on how horizontal the line is. What's nice is that this 167 | # is exactly the behavior we want and it gets up back on the line! 168 | 169 | if old_cx_normal != None: old_cx_normal = (cx_normal * MIXING_RATE) + (old_cx_normal * (1.0 - MIXING_RATE)) 170 | else: old_cx_normal = cx_normal 171 | return old_cx_normal 172 | 173 | # Solve: THROTTLE_CUT_OFF_RATE = pow(sin(90 +/- THROTTLE_CUT_OFF_ANGLE), x) for x... 174 | # -> sin(90 +/- THROTTLE_CUT_OFF_ANGLE) = cos(THROTTLE_CUT_OFF_ANGLE) 175 | t_power = math.log(THROTTLE_CUT_OFF_RATE) / math.log(math.cos(math.radians(THROTTLE_CUT_OFF_ANGLE))) 176 | 177 | def figure_out_my_throttle(steering): # steering -> [0:180] 178 | 179 | # pow(sin()) of the steering angle is only non-zero when driving straight... e.g. steering ~= 90 180 | t_result = math.pow(math.sin(math.radians(max(min(steering, 179.99), 0.0))), t_power) 181 | 182 | return (t_result * THROTTLE_GAIN) + THROTTLE_OFFSET 183 | 184 | 185 | def steer(throttle, steering): 186 | throttle = THROTTLE_SERVO_MIN_US + ((throttle * (THROTTLE_SERVO_MAX_US - THROTTLE_SERVO_MIN_US + 1)) / 101) 187 | steering = STEERING_SERVO_MIN_US + ((steering * (STEERING_SERVO_MAX_US - STEERING_SERVO_MIN_US + 1)) / 181) 188 | steering = steering + (1500-steering)* steering_gain 189 | print("Steering: ",round(steering), "Throttle: ", round(throttle)) 190 | uart.write(str(round(steering))) # send to the Arduino. It looks for channel outputs in order, seperated by a "," and ended with a "\n" 191 | uart.write(",") 192 | uart.write(str(round(throttle))) 193 | uart.write("\n") 194 | 195 | 196 | # 197 | # Camera Control Code 198 | # 199 | 200 | sensor.reset() 201 | sensor.set_pixformat(sensor.RGB565 if COLOR_LINE_FOLLOWING else sensor.GRAYSCALE) 202 | sensor.set_framesize(FRAME_SIZE) 203 | sensor.set_vflip(True) 204 | sensor.set_hmirror(True) 205 | sensor.set_windowing((int((sensor.width() / 2) - ((sensor.width() / 2) * FRAME_WIDE)), int(sensor.height() * (1.0 - FRAME_REGION)), \ 206 | int((sensor.width() / 2) + ((sensor.width() / 2) * FRAME_WIDE)), int(sensor.height() * FRAME_REGION))) 207 | sensor.skip_frames(time = 200) 208 | if COLOR_LINE_FOLLOWING: sensor.set_auto_gain(False) 209 | if COLOR_LINE_FOLLOWING: sensor.set_auto_whitebal(False) 210 | clock = time.clock() 211 | #sensor.set_auto_exposure(False, \ 212 | # exposure_us = 300) 213 | 214 | 215 | ########### 216 | # Loop 217 | ########### 218 | 219 | old_time = pyb.millis() 220 | 221 | throttle_old_result = None 222 | throttle_i_output = 0 223 | throttle_output = THROTTLE_OFFSET 224 | 225 | steering_old_result = None 226 | steering_i_output = 0 227 | steering_output = STEERING_OFFSET 228 | 229 | while True: 230 | clock.tick() 231 | switch = switch_pin.value() # get value, 0 or 1 232 | if switch == 1 or not RC_control: # Teensy says you're in MV mode 233 | img = sensor.snapshot().lens_corr(strength = 2.8, zoom = 1) # lens correction for fisheye lens 234 | img.rotation_corr(corners = TARGET_POINTS) 235 | img.binary(COLOR_HIGH_LIGHT_THRESHOLDS if COLOR_LINE_FOLLOWING else GRAYSCALE_HIGH_LIGHT_THRESHOLDS, zero = True) 236 | img.histeq() 237 | 238 | if BINARY_VIEW: img = img.binary(COLOR_THRESHOLDS if COLOR_LINE_FOLLOWING else GRAYSCALE_THRESHOLDS) 239 | if BINARY_VIEW: img.erode(1, threshold = 5).dilate(1, threshold = 1) 240 | 241 | # We call get regression below to get a robust linear regression of the field of view. 242 | # This returns a line object which we can use to steer the robocar. 243 | line = img.get_regression(([(50, 100, -128, 127, -128, 127)] if BINARY_VIEW else COLOR_THRESHOLDS) if COLOR_LINE_FOLLOWING \ 244 | else ([(127, 255)] if BINARY_VIEW else GRAYSCALE_THRESHOLDS), \ 245 | area_threshold = AREA_THRESHOLD, pixels_threshold = PIXELS_THRESHOLD, \ 246 | robust = False) 247 | 248 | print_string = "" 249 | if line and (line.magnitude() >= MAG_THRESHOLD): 250 | img.draw_line(line.line(), color = (256, 0, 0) if COLOR_LINE_FOLLOWING else 127) 251 | 252 | new_time = pyb.millis() 253 | delta_time = new_time - old_time 254 | old_time = new_time 255 | 256 | # 257 | # Figure out steering and do steering PID 258 | # 259 | 260 | steering_new_result = figure_out_my_steering(line, img) 261 | steering_delta_result = (steering_new_result - steering_old_result) if (steering_old_result != None) else 0 262 | steering_old_result = steering_new_result 263 | 264 | steering_p_output = steering_new_result # Standard PID Stuff here... nothing particularly interesting :) 265 | steering_i_output = max(min(steering_i_output + steering_new_result, STEERING_I_MAX), STEERING_I_MIN) 266 | steering_d_output = ((steering_delta_result * 1000) / delta_time) if delta_time else 0 267 | steering_pid_output = (STEERING_P_GAIN * steering_p_output) + \ 268 | (STEERING_I_GAIN * steering_i_output) + \ 269 | (STEERING_D_GAIN * steering_d_output) 270 | 271 | # Steering goes from [-90,90] but we need to output [0,180] for the servos. 272 | steering_output = STEERING_OFFSET + max(min(round(steering_pid_output), 180 - STEERING_OFFSET), STEERING_OFFSET - 180) 273 | 274 | # 275 | # Figure out throttle and do throttle PID 276 | # 277 | 278 | throttle_new_result = figure_out_my_throttle(steering_output) 279 | throttle_delta_result = (throttle_new_result - throttle_old_result) if (throttle_old_result != None) else 0 280 | throttle_old_result = throttle_new_result 281 | 282 | throttle_p_output = throttle_new_result # Standard PID Stuff here... nothing particularly interesting :) 283 | throttle_i_output = max(min(throttle_i_output + throttle_new_result, THROTTLE_I_MAX), THROTTLE_I_MIN) 284 | throttle_d_output = ((throttle_delta_result * 1000) / delta_time) if delta_time else 0 285 | throttle_pid_output = (THROTTLE_P_GAIN * throttle_p_output) + \ 286 | (THROTTLE_I_GAIN * throttle_i_output) + \ 287 | (THROTTLE_D_GAIN * throttle_d_output) 288 | 289 | # Throttle goes from 0% to 100%. 290 | throttle_output = max(min(round(throttle_pid_output), 100), 0) 291 | 292 | print_string = "Line Ok - throttle %d, steering %d - line t: %d, r: %d" % \ 293 | (throttle_output , steering_output, line.theta(), line.rho()) 294 | 295 | else: 296 | print_string = "Line Lost - throttle %d, steering %d" % (throttle_output , steering_output) 297 | 298 | # blink LED green 299 | 300 | now = pyb.millis() 301 | if now > led_time + 1000: # switch LED every second 302 | if led_state == True: 303 | led_control(0) # turn off LED 304 | led_state = False 305 | else: 306 | led_control(2) # turn on Green LED 307 | led_state = True 308 | led_time = now # reset time counter 309 | 310 | steer(throttle_output, steering_output) 311 | print("FPS %f - %s" % (clock.fps(), print_string)) 312 | else: 313 | # blink LED blue 314 | now = pyb.millis() 315 | if now > led_time + 1000: # switch LED every second 316 | print("Under RC control") 317 | if led_state == True: 318 | led_control(0) # turn off LED 319 | led_state = False 320 | else: 321 | led_control(4) # turn on Blue LED 322 | led_state = True 323 | led_time = now # reset time counter 324 | -------------------------------------------------------------------------------- /ScanseRover.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import itertools 4 | import sys 5 | import time 6 | from sweeppy import Sweep 7 | import math 8 | import serial 9 | 10 | 11 | angle_offset = 0 # this compensates for the Lidar being placed in a rotated position 12 | start = time.time() 13 | stop = False 14 | 15 | 16 | PORT_NAME = '/dev/ttyUSB0' # this is for the Lidar 17 | # Teensy_Port = serial.Serial("/dev/ttyACM0", baudrate=19200, timeout=1) 18 | 19 | def constrain(val, min_val, max_val): 20 | if val < min_val: return min_val 21 | if val > max_val: return max_val 22 | return val 23 | 24 | 25 | def scan(): 26 | global stop 27 | point_list = [] 28 | slope_list = [] 29 | while True: 30 | print('Recording measurements... Press Crl+C to stop.') 31 | lasttime = time.time() 32 | with Sweep(dev) as sweep: 33 | speed = sweep.get_motor_speed() 34 | rate = sweep.get_sample_rate() 35 | print('Motor Speed: {} Hz'.format(speed)) 36 | print('Sample Rate: {} Hz'.format(rate)) 37 | 38 | # Starts scanning as soon as the motor is read 39 | sweep.start_scanning() 40 | 41 | # get_scans is coroutine-based generator lazily returning scans ad infinitum 42 | for scan in itertools.islice(sweep.get_scans(), 3): 43 | print(scan) 44 | if time.time() < (lasttime + 0.1): 45 | # [0] -> New 46 | # [1] -> Quality 47 | # [2] -> Angle (0 - 360) 48 | # [3] -> Distance in mm 49 | print("time: ", time.time()) 50 | #print("lasttime: ", lasttime) 51 | # if((measurment[3] > 100) and (measurment[3]<2500) and (measurment[1]>2) and ((measurment[2] < 135) or (225 < measurment[2]))): 52 | # x_pos = math.cos(math.radians(measurment[2])) * measurment[3] # in mm 53 | # y_pos = math.sin(math.radians(measurment[2])) * measurment[3] # in mm 54 | # point_list.append((x_pos, y_pos)) 55 | else: 56 | if len(point_list) > 2: 57 | for i in range(len(point_list)): 58 | this_point = point_list[i] 59 | for j in range(len(point_list) - 1 - i): 60 | other_point = point_list[1 + i] 61 | mx = this_point[0] - other_point[0] 62 | my = this_point[1] - other_point[1] 63 | temp = math.atan2(my,mx) 64 | if not math.isnan(temp) and not math.isinf(temp): 65 | slope_list.append(temp) 66 | slope_list = sorted(slope_list) 67 | median = math.degrees(slope_list[len(slope_list)/2]) 68 | median = median % 180 # forcing output to be between 0 and 179 69 | if median < 0: 70 | median += 180 71 | slope_sum = 0 72 | for j in range(len(slope_list)): 73 | temp2 = slope_list[j] % 180 74 | if temp2 < 0: 75 | temp2 += 180 76 | temp2 = temp2 - median 77 | temp2 = temp2*temp2 78 | slope_sum = slope_sum + temp2 79 | slope_sum = slope_sum / len(slope_list) 80 | slope_sum = math.sqrt(slope_sum) 81 | # print("Slope sum: ",slope_sum) 82 | median = 10 * (1/math.tan(math.radians(median))) 83 | # Teensy_Port.write("%f\n" % median) 84 | print (median) 85 | point_list = [] 86 | slope_list = [] 87 | lasttime = time.time() 88 | 89 | def run(): 90 | '''Main function''' 91 | time.sleep(1) 92 | print("Starting...") 93 | try: 94 | scan() 95 | except KeyboardInterrupt: 96 | stop = True 97 | 98 | if __name__ == '__main__': 99 | run 100 | -------------------------------------------------------------------------------- /TeensyShield/TeensyShield.ino: -------------------------------------------------------------------------------- 1 | // This file is part of the OpenMV project. 2 | // Copyright (c) 2013-2017 Ibrahim Abdelkader & Kwabena W. Agyeman 3 | // Updated for Teensy shield by Chris Anderson: https://circuits.io/circuits/5453306-openmv-teensy-shield 4 | // This work is licensed under the MIT license, see the file LICENSE for details. 5 | 6 | #include 7 | 8 | #define SERIAL_RX_PIN 0 9 | #define SERIAL_TX_PIN 1 10 | #define THROTTLE_SERVO_PIN 20 11 | #define STEERING_SERVO_PIN 21 12 | #define RC_THROTTLE_PIN 4 13 | #define RC_STEERING_PIN 3 14 | #define RC_SWITCH_PIN 5 15 | #define LED_PIN 13 16 | #define VOLTAGE_PIN 15 17 | 18 | #define SERIAL_BAUD_RATE 19200 19 | 20 | #define RC_THROTTLE_SERVO_REFRESH_RATE 20000UL // in us 21 | #define SERIAL_THROTTLE_SERVO_REFRESH_RATE 1000000UL // in us 22 | #define RC_THROTTLE_DEAD_ZONE_MIN 1400UL // in us 23 | #define RC_THROTTLE_DEAD_ZONE_MAX 1600UL // in us 24 | 25 | #define RC_STEERING_SERVO_REFRESH_RATE 20000UL // in us 26 | #define SERIAL_STEERING_SERVO_REFRESH_RATE 1000000UL // in us 27 | #define RC_STEERING_DEAD_ZONE_MIN 1400UL // in us 28 | #define RC_STEERING_DEAD_ZONE_MAX 1600UL // in us 29 | 30 | Servo throttle_servo, steering_servo; 31 | 32 | unsigned long last_microseconds; 33 | bool last_rc_throttle_pin_state, last_rc_steering_pin_state; 34 | unsigned long last_rc_throttle_microseconds, last_rc_steering_microseconds; 35 | unsigned long rc_throttle_servo_pulse_length = 0, rc_steering_servo_pulse_length = 0; 36 | unsigned long rc_throttle_servo_pulse_refreshed = 0, rc_steering_servo_pulse_refreshed = 0; 37 | float voltage; 38 | float throttle_compensation = 1; 39 | 40 | char serial_buffer[16] = {}; 41 | unsigned long serial_throttle_servo_pulse_length = 0, serial_steering_servo_pulse_length = 0; 42 | unsigned long serial_throttle_servo_pulse_refreshed = 0, serial_steering_servo_pulse_refreshed = 0; 43 | 44 | void setup() 45 | { 46 | Serial.begin(SERIAL_BAUD_RATE); 47 | Serial1.begin(SERIAL_BAUD_RATE); 48 | pinMode(LED_PIN, OUTPUT); 49 | pinMode(RC_STEERING_PIN, INPUT); 50 | pinMode(RC_THROTTLE_PIN, INPUT); 51 | pinMode(VOLTAGE_PIN, INPUT); 52 | last_microseconds = micros(); 53 | last_rc_throttle_pin_state = digitalRead(RC_THROTTLE_PIN) == HIGH; 54 | last_rc_steering_pin_state = digitalRead(RC_STEERING_PIN) == HIGH; 55 | last_rc_steering_pin_state = digitalRead(RC_SWITCH_PIN) == HIGH; 56 | last_rc_throttle_microseconds = last_microseconds; 57 | last_rc_steering_microseconds = last_microseconds; 58 | digitalWrite(LED_PIN,HIGH); 59 | } 60 | 61 | void loop() 62 | { 63 | unsigned long microseconds = micros(); 64 | bool rc_throttle_pin_state = digitalRead(RC_THROTTLE_PIN) == HIGH; 65 | bool rc_steering_pin_state = digitalRead(RC_STEERING_PIN) == HIGH; 66 | bool rc_steering_pin_state = digitalRead(RC_SWITCH_PIN) == HIGH; 67 | voltage = analogRead(VOLTAGE_PIN); 68 | voltage = voltage/64.8; //divisor for the resistor divider 69 | // Serial.print("Voltage: "); 70 | // Serial.println(voltage); 71 | throttle_compensation = 7.4/voltage; 72 | if(rc_throttle_pin_state && (!last_rc_throttle_pin_state)) // rising edge 73 | { 74 | last_rc_throttle_microseconds = microseconds; 75 | } 76 | 77 | if((!rc_throttle_pin_state) && last_rc_throttle_pin_state) // falling edge 78 | { 79 | unsigned long temp = microseconds - last_rc_throttle_microseconds; 80 | 81 | if(!rc_throttle_servo_pulse_length) 82 | { 83 | rc_throttle_servo_pulse_length = temp; 84 | } 85 | else 86 | { 87 | rc_throttle_servo_pulse_length = ((rc_throttle_servo_pulse_length * 3) + temp) >> 2; 88 | } 89 | 90 | rc_throttle_servo_pulse_refreshed = microseconds; 91 | } 92 | 93 | if(rc_throttle_servo_pulse_length // zero servo if not refreshed 94 | && ((microseconds - rc_throttle_servo_pulse_refreshed) > (2UL * RC_THROTTLE_SERVO_REFRESH_RATE))) 95 | { 96 | rc_throttle_servo_pulse_length = 0; 97 | } 98 | 99 | if(rc_steering_pin_state && (!last_rc_steering_pin_state)) // rising edge 100 | { 101 | last_rc_steering_microseconds = microseconds; 102 | } 103 | 104 | if((!rc_steering_pin_state) && last_rc_steering_pin_state) // falling edge 105 | { 106 | unsigned long temp = microseconds - last_rc_steering_microseconds; 107 | 108 | if(!rc_steering_servo_pulse_length) 109 | { 110 | rc_steering_servo_pulse_length = temp; 111 | } 112 | else 113 | { 114 | rc_steering_servo_pulse_length = ((rc_steering_servo_pulse_length * 3) + temp) >> 2; 115 | // Serial.print("Steer: "); 116 | // Serial.println(rc_steering_servo_pulse_length); 117 | } 118 | 119 | rc_steering_servo_pulse_refreshed = microseconds; 120 | } 121 | 122 | if(rc_steering_servo_pulse_length // zero servo if not refreshed 123 | && ((microseconds - rc_steering_servo_pulse_refreshed) > (2UL * RC_STEERING_SERVO_REFRESH_RATE))) 124 | { 125 | rc_steering_servo_pulse_length = 0; 126 | } 127 | 128 | last_microseconds = microseconds; 129 | last_rc_throttle_pin_state = rc_throttle_pin_state; 130 | last_rc_steering_pin_state = rc_steering_pin_state; 131 | 132 | while(Serial.available()) 133 | { 134 | int m = Serial.read(); 135 | Serial1.write(m); // echo USB serial to the OpenMV 136 | } 137 | while(Serial1.available()) 138 | { 139 | int c = Serial1.read(); 140 | Serial.write(c); //echo the incoming serial stream from the OpenMV 141 | memmove(serial_buffer, serial_buffer + 1, sizeof(serial_buffer) - 2); 142 | serial_buffer[sizeof(serial_buffer) - 2] = c; 143 | 144 | if(c == '\n') 145 | { 146 | unsigned long serial_throttle_servo_pulse_length_tmp, serial_steering_servo_pulse_length_tmp; 147 | 148 | if(sscanf(serial_buffer, "{%lu,%lu}", &serial_throttle_servo_pulse_length_tmp, &serial_steering_servo_pulse_length_tmp) == 2) 149 | { 150 | if(!serial_throttle_servo_pulse_length) 151 | { 152 | serial_throttle_servo_pulse_length = serial_throttle_servo_pulse_length_tmp; 153 | } 154 | else 155 | { 156 | serial_throttle_servo_pulse_length = ((serial_throttle_servo_pulse_length * 3) + serial_throttle_servo_pulse_length_tmp) >> 2; 157 | } 158 | 159 | serial_throttle_servo_pulse_refreshed = microseconds; 160 | 161 | if(!serial_steering_servo_pulse_length) 162 | { 163 | serial_steering_servo_pulse_length = serial_steering_servo_pulse_length_tmp; 164 | } 165 | else 166 | { 167 | serial_steering_servo_pulse_length = ((serial_steering_servo_pulse_length * 3) + serial_steering_servo_pulse_length_tmp) >> 2; 168 | } 169 | 170 | serial_steering_servo_pulse_refreshed = microseconds; 171 | 172 | digitalWrite(LED_PIN, (digitalRead(LED_PIN) == HIGH) ? LOW : HIGH); 173 | } 174 | else 175 | { 176 | serial_throttle_servo_pulse_length = 0; 177 | serial_steering_servo_pulse_length = 0; 178 | } 179 | } 180 | } 181 | 182 | if(serial_throttle_servo_pulse_length // zero servo if not refreshed 183 | && ((microseconds - serial_throttle_servo_pulse_refreshed) > (2UL * SERIAL_THROTTLE_SERVO_REFRESH_RATE))) 184 | { 185 | serial_throttle_servo_pulse_length = 0; 186 | } 187 | 188 | if(serial_steering_servo_pulse_length // zero servo if not refreshed 189 | && ((microseconds - serial_steering_servo_pulse_refreshed) > (2UL * SERIAL_STEERING_SERVO_REFRESH_RATE))) 190 | { 191 | serial_steering_servo_pulse_length = 0; 192 | } 193 | 194 | if(rc_steering_servo_pulse_length) 195 | { 196 | if(!steering_servo.attached()) 197 | { 198 | throttle_servo.attach(THROTTLE_SERVO_PIN); 199 | steering_servo.attach(STEERING_SERVO_PIN); 200 | } 201 | 202 | if(serial_steering_servo_pulse_length) 203 | { 204 | if((rc_throttle_servo_pulse_length < RC_THROTTLE_DEAD_ZONE_MIN) 205 | || (rc_throttle_servo_pulse_length > RC_THROTTLE_DEAD_ZONE_MAX)) 206 | { 207 | throttle_servo.writeMicroseconds(serial_throttle_servo_pulse_length); 208 | Serial.print("Throttle: "); 209 | Serial.println(serial_throttle_servo_pulse_length); 210 | } 211 | else 212 | { 213 | throttle_servo.writeMicroseconds(1500); 214 | } 215 | 216 | if((rc_steering_servo_pulse_length < RC_STEERING_DEAD_ZONE_MIN) 217 | || (rc_steering_servo_pulse_length > RC_STEERING_DEAD_ZONE_MAX)) 218 | { 219 | steering_servo.writeMicroseconds(rc_steering_servo_pulse_length); 220 | } 221 | else 222 | { 223 | steering_servo.writeMicroseconds(serial_steering_servo_pulse_length); 224 | } 225 | } 226 | else 227 | { 228 | throttle_servo.writeMicroseconds(rc_throttle_servo_pulse_length); 229 | steering_servo.writeMicroseconds(rc_steering_servo_pulse_length); 230 | } 231 | } 232 | else if(steering_servo.attached()) 233 | { 234 | throttle_servo.detach(); 235 | steering_servo.detach(); 236 | } 237 | } 238 | -------------------------------------------------------------------------------- /bwlinefollowingArduino.py: -------------------------------------------------------------------------------- 1 | # Black Grayscale Line Following Example, updated for PID 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | 9 | import sensor, image, math, time, pyb 10 | from pyb import LED 11 | from pyb import UART 12 | uart = UART(3, 9600) # no need to go faster than this. Slower = solid comms 13 | 14 | # Tracks a white line. Use the Machine Vision threshold setting tool in the IDE to find the right values for your line (Black or White) 15 | GRAYSCALE_THRESHOLD = [(200, 255)] 16 | 17 | cruise_speed = 0 # how fast should the car drive, range from 1000 to 2000 18 | steer_direction = -1 # use this to revers the steering if your car goes in the wrong direction 19 | steering_gain = 1.1 # calibration for your car's steering sensitivity 20 | steering_center = 80 # set to your car servo's center point 21 | kp = 0.4 # P term of the PID 22 | ki = 0.0 # I term of the PID 23 | kd = 0.3 # D term of the PID 24 | 25 | red_led = LED(1) # in case you want to do some signalling with LEDs 26 | green_led = LED(2) 27 | blue_led = LED(3) 28 | ir_led = LED(4) 29 | 30 | old_error = 0 31 | measured_angle = 0 32 | set_angle = 90 # this is the desired steering angle (straight ahead) 33 | p_term = 0 # initialize the terms to zero 34 | i_term = 0 35 | d_term = 0 36 | old_time = pyb.millis() 37 | 38 | def led_control(x): 39 | if (x&1)==0: red_led.off() 40 | elif (x&1)==1: red_led.on() 41 | if (x&2)==0: green_led.off() 42 | elif (x&2)==2: green_led.on() 43 | if (x&4)==0: blue_led.off() 44 | elif (x&4)==4: blue_led.on() 45 | if (x&8)==0: ir_led.off() 46 | elif (x&8)==8: ir_led.on() 47 | 48 | def constrain(value, min, max): 49 | if value < min : 50 | return min 51 | if value > max : 52 | return max 53 | else: 54 | return value 55 | 56 | def steer(angle): 57 | global steering_gain, cruise_speed, steering_center 58 | angle = int(round((angle+steering_center)*steering_gain)) 59 | constrain(angle, 0, 180) 60 | ch1 = str(angle) # must convert to text to send via Serial 61 | ch2 = str(cruise_speed) # send throttle data, too 62 | print(angle) 63 | uart.write(ch1) # send to the Arduino. It looks for channel outputs in order, seperated by a "," and ended with a "\n" 64 | uart.write(",") 65 | uart.write(ch2) 66 | uart.write("\n") 67 | 68 | def update_pid(): 69 | global old_time, old_error, measured_angle, set_angle 70 | global p_term, i_term, d_term 71 | now = pyb.millis() 72 | dt = now - old_time 73 | error = set_angle - measured_angle 74 | de = error - old_error 75 | 76 | p_term = kp * error 77 | i_term += ki * error 78 | i_term = constrain(i_term, 0, 100) 79 | d_term = (de / dt) * kd 80 | 81 | old_error = error 82 | output = steer_direction * (p_term + i_term + d_term) 83 | output = constrain(output, -50, 50) 84 | return output 85 | 86 | 87 | 88 | 89 | # Each roi is (x, y, w, h). The line detection algorithm will try to find the 90 | # centroid of the largest blob in each roi. The x position of the centroids 91 | # will then be averaged with different weights where the most weight is assigned 92 | # to the roi near the bottom of the image and less to the next roi and so on. 93 | ROIS = [ # [ROI, weight] 94 | (0, 100, 160, 20, 0.1), # You'll need to tweak the weights for your app 95 | (0, 050, 160, 20, 0.3), # depending on how your robot is setup. 96 | (0, 000, 160, 20, 0.7) 97 | ] 98 | 99 | # Compute the weight divisor (we're computing this so you don't have to make weights add to 1). 100 | weight_sum = 0 101 | for r in ROIS: weight_sum += r[4] # r[4] is the roi weight. 102 | 103 | # Camera setup... 104 | sensor.reset() # Initialize the camera sensor. 105 | sensor.set_pixformat(sensor.GRAYSCALE) # use grayscale. 106 | sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed. 107 | sensor.skip_frames(30) # Let new settings take affect. 108 | sensor.set_auto_gain(False) # must be turned off for color tracking 109 | sensor.set_auto_whitebal(False) # must be turned off for color tracking 110 | clock = time.clock() # Tracks FPS. 111 | 112 | while(True): 113 | clock.tick() # Track elapsed milliseconds between snapshots(). 114 | img = sensor.snapshot() # Take a picture and return the image. 115 | 116 | centroid_sum = 0 117 | for r in ROIS: 118 | blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True) # r[0:4] is roi tuple. 119 | if blobs: 120 | # Find the index of the blob with the most pixels. 121 | most_pixels = 0 122 | largest_blob = 0 123 | for i in range(len(blobs)): 124 | if blobs[i].pixels() > most_pixels: 125 | most_pixels = blobs[i].pixels() 126 | largest_blob = i 127 | 128 | # Draw a rect around the blob. 129 | img.draw_rectangle(blobs[largest_blob].rect()) 130 | img.draw_cross(blobs[largest_blob].cx(), 131 | blobs[largest_blob].cy()) 132 | 133 | centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight. 134 | 135 | center_pos = (centroid_sum / weight_sum) # Determine center of line. 136 | 137 | # Convert the center_pos to a deflection angle. We're using a non-linear 138 | # operation so that the response gets stronger the farther off the line we 139 | # are. Non-linear operations are good to use on the output of algorithms 140 | # like this to cause a response "trigger". 141 | deflection_angle = 0 142 | # The 80 is from half the X res, the 60 is from half the Y res. The 143 | # equation below is just computing the angle of a triangle where the 144 | # opposite side of the triangle is the deviation of the center position 145 | # from the center and the adjacent side is half the Y res. This limits 146 | # the angle output to around -45 to 45. (It's not quite -45 and 45). 147 | deflection_angle = -math.atan((center_pos-80)/60) 148 | 149 | # Convert angle in radians to degrees. 150 | deflection_angle = math.degrees(deflection_angle) 151 | 152 | # Now you have an angle telling you how much to turn the robot by which 153 | # incorporates the part of the line nearest to the robot and parts of 154 | # the line farther away from the robot for a better prediction. 155 | # print("Turn Angle: %f" % deflection_angle) 156 | now = pyb.millis() 157 | if now > old_time + 1.0 : # time has passed since last measurement 158 | measured_angle = deflection_angle + 90 159 | steer_angle = update_pid() # send error off to the PID to get a correction command 160 | old_time = now # reset clock 161 | steer (steer_angle) # this is the line that does all the work ;-) 162 | print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle)) 163 | -------------------------------------------------------------------------------- /bwlinefollowingRC.py: -------------------------------------------------------------------------------- 1 | # Black Grayscale Line Following Example, updated for PID 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import Servo 10 | from pyb import LED 11 | 12 | # Tracks a black line. Use [(128, 255)] for a tracking a white line. 13 | GRAYSCALE_THRESHOLD = [(200, 255)] 14 | 15 | s1 = Servo(1) # P7 Motor 16 | s2 = Servo(2) # P8 Steering 17 | print (s1.calibration()) # show throttle servo calibration 18 | cruise_speed = 0 # how fast should the car drive, range from 1000 to 2000 19 | steer_direction = -1 20 | steering_gain = 400 21 | kp = 0.4 # P term of the PID 22 | ki = 0.0 # I term of the PID 23 | kd = 0.3 # D term of the PID 24 | 25 | red_led = LED(1) 26 | green_led = LED(2) 27 | blue_led = LED(3) 28 | ir_led = LED(4) 29 | 30 | old_error = 0 31 | measured_angle = 0 32 | set_angle = 90 # this is the desired steering angle (straight ahead) 33 | p_term = 0 34 | i_term = 0 35 | d_term = 0 36 | old_time = pyb.millis() 37 | 38 | def led_control(x): 39 | if (x&1)==0: red_led.off() 40 | elif (x&1)==1: red_led.on() 41 | if (x&2)==0: green_led.off() 42 | elif (x&2)==2: green_led.on() 43 | if (x&4)==0: blue_led.off() 44 | elif (x&4)==4: blue_led.on() 45 | if (x&8)==0: ir_led.off() 46 | elif (x&8)==8: ir_led.on() 47 | 48 | def constrain(value, min, max): 49 | if value < min : 50 | return min 51 | if value > max : 52 | return max 53 | else: 54 | return value 55 | 56 | def steer(angle): 57 | global steering_gain 58 | s1.angle(angle) # steer 59 | pyb.delay(10) 60 | 61 | def update_pid(): 62 | global old_time, old_error, measured_angle, set_angle 63 | global p_term, i_term, d_term 64 | now = pyb.millis() 65 | dt = now - old_time 66 | error = set_angle - measured_angle 67 | de = error - old_error 68 | 69 | p_term = kp * error 70 | i_term += ki * error 71 | i_term = constrain(i_term, 0, 100) 72 | d_term = (de / dt) * kd 73 | 74 | old_error = error 75 | output = steer_direction * (p_term + i_term + d_term) 76 | output = constrain(output, -50, 50) 77 | return output 78 | 79 | 80 | 81 | 82 | # Each roi is (x, y, w, h). The line detection algorithm will try to find the 83 | # centroid of the largest blob in each roi. The x position of the centroids 84 | # will then be averaged with different weights where the most weight is assigned 85 | # to the roi near the bottom of the image and less to the next roi and so on. 86 | ROIS = [ # [ROI, weight] 87 | (0, 100, 160, 20, 0.1), # You'll need to tweak the weights for your app 88 | (0, 050, 160, 20, 0.3), # depending on how your robot is setup. 89 | (0, 000, 160, 20, 0.7) 90 | ] 91 | 92 | # Compute the weight divisor (we're computing this so you don't have to make weights add to 1). 93 | weight_sum = 0 94 | for r in ROIS: weight_sum += r[4] # r[4] is the roi weight. 95 | 96 | # Camera setup... 97 | sensor.reset() # Initialize the camera sensor. 98 | sensor.set_pixformat(sensor.GRAYSCALE) # use grayscale. 99 | sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed. 100 | sensor.skip_frames(30) # Let new settings take affect. 101 | sensor.set_auto_gain(False) # must be turned off for color tracking 102 | sensor.set_auto_whitebal(False) # must be turned off for color tracking 103 | clock = time.clock() # Tracks FPS. 104 | 105 | while(True): 106 | clock.tick() # Track elapsed milliseconds between snapshots(). 107 | img = sensor.snapshot() # Take a picture and return the image. 108 | 109 | centroid_sum = 0 110 | for r in ROIS: 111 | blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True) # r[0:4] is roi tuple. 112 | if blobs: 113 | # Find the index of the blob with the most pixels. 114 | most_pixels = 0 115 | largest_blob = 0 116 | for i in range(len(blobs)): 117 | if blobs[i].pixels() > most_pixels: 118 | most_pixels = blobs[i].pixels() 119 | largest_blob = i 120 | 121 | # Draw a rect around the blob. 122 | img.draw_rectangle(blobs[largest_blob].rect()) 123 | img.draw_cross(blobs[largest_blob].cx(), 124 | blobs[largest_blob].cy()) 125 | 126 | centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight. 127 | 128 | center_pos = (centroid_sum / weight_sum) # Determine center of line. 129 | 130 | # Convert the center_pos to a deflection angle. We're using a non-linear 131 | # operation so that the response gets stronger the farther off the line we 132 | # are. Non-linear operations are good to use on the output of algorithms 133 | # like this to cause a response "trigger". 134 | deflection_angle = 0 135 | # The 80 is from half the X res, the 60 is from half the Y res. The 136 | # equation below is just computing the angle of a triangle where the 137 | # opposite side of the triangle is the deviation of the center position 138 | # from the center and the adjacent side is half the Y res. This limits 139 | # the angle output to around -45 to 45. (It's not quite -45 and 45). 140 | deflection_angle = -math.atan((center_pos-80)/60) 141 | 142 | # Convert angle in radians to degrees. 143 | deflection_angle = math.degrees(deflection_angle) 144 | 145 | # Now you have an angle telling you how much to turn the robot by which 146 | # incorporates the part of the line nearest to the robot and parts of 147 | # the line farther away from the robot for a better prediction. 148 | # print("Turn Angle: %f" % deflection_angle) 149 | now = pyb.millis() 150 | if now > old_time + 1.0 : # time has passed since last measurement 151 | measured_angle = deflection_angle + 90 152 | steer_angle = update_pid() 153 | old_time = now 154 | steer (steer_angle) 155 | print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle)) 156 | -------------------------------------------------------------------------------- /colorLinearRegression.py: -------------------------------------------------------------------------------- 1 | # Color Line Following Example with PID Steering 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import Servo 10 | from pyb import LED 11 | from pyb import UART 12 | from pyb import Pin 13 | 14 | 15 | switch_pin = Pin('P7', Pin.IN, Pin.PULL_DOWN) 16 | 17 | uart = UART(3, 19200) # Use UART 3 (pins 4 and 5). No need to go faster than this. Slower = solid comms 18 | 19 | 20 | THRESHOLD = (0, 100, -35, 127, -128, -30) # blue threshold 21 | # THRESHOLD = (0, 100, 35, 127, -15, 127) # red threshold 22 | 23 | TARGET_POINTS = [(70, 50), # (x, y), clockwise from top left 24 | (285, 50), 25 | (319, 239), 26 | (1,239)] 27 | 28 | 29 | cruise_speed = 1575 # how fast should the car drive, range from 1000 to 2000. 1500 is stopped. 1575 is slowest it can go 30 | steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction 31 | steering_gain = 1.1 # calibration for your car's steering sensitivity 32 | steering_center = 80 # set to your car servo's center point 33 | kp = 0.4 # P term of the PID 34 | ki = 0.0 # I term of the PID 35 | kd = 0.3 # D term of the PID 36 | RC_control = False # Set to false if you're just benchtop testing 37 | 38 | red_led = LED(1) 39 | green_led = LED(2) 40 | blue_led = LED(3) 41 | ir_led = LED(4) 42 | led_state = True 43 | led_time = pyb.millis() 44 | 45 | old_error = 0 46 | measured_angle = 0 47 | gain = 5 48 | set_angle = 90 # this is the desired steering angle (straight ahead) 49 | p_term = 0 50 | i_term = 0 51 | d_term = 0 52 | old_time = pyb.millis() 53 | temp = "" 54 | 55 | def led_control(x): 56 | if (x&1)==0: red_led.off() 57 | elif (x&1)==1: red_led.on() 58 | if (x&2)==0: green_led.off() 59 | elif (x&2)==2: green_led.on() 60 | if (x&4)==0: blue_led.off() 61 | elif (x&4)==4: blue_led.on() 62 | if (x&8)==0: ir_led.off() 63 | elif (x&8)==8: ir_led.on() 64 | 65 | def constrain(value, min, max): 66 | if value < min : 67 | return min 68 | if value > max : 69 | return max 70 | else: 71 | return value 72 | 73 | def steer(angle): 74 | global steering_gain, cruise_speed, steering_center 75 | angle = int(round((angle+steering_center)*steering_gain)) 76 | constrain(angle, 0, 180) 77 | ch1 = str(angle) # must convert to text to send via Serial 78 | ch2 = str(cruise_speed) # send throttle data, too 79 | # print(angle) 80 | uart.write(ch1) # send to the Arduino. It looks for channel outputs in order, seperated by a "," and ended with a "\n" 81 | uart.write(",") 82 | uart.write(ch2) 83 | uart.write("\n") 84 | 85 | def update_pid(): 86 | global old_time, old_error, measured_angle, set_angle 87 | global p_term, i_term, d_term 88 | now = pyb.millis() 89 | dt = now - old_time 90 | error = set_angle - measured_angle 91 | de = error - old_error 92 | 93 | p_term = kp * error 94 | i_term += ki * error 95 | i_term = constrain(i_term, 0, 100) 96 | d_term = (de / dt) * kd 97 | 98 | old_error = error 99 | output = steering_direction * (p_term + i_term + d_term) 100 | output = constrain(output, -50, 50) 101 | return output 102 | 103 | 104 | 105 | # Camera setup... 106 | clock = time.clock() # Tracks FPS. 107 | sensor.reset() # Initialize the camera sensor. 108 | sensor.__write_reg(0x6B, 0x22) # switches camera into advanced calibration mode. See this for more: http://forums.openmv.io/viewtopic.php?p=1358#p1358 109 | sensor.set_pixformat(sensor.RGB565) 110 | sensor.set_framesize(sensor.QVGA) # use QVGA for speed. 111 | sensor.set_vflip(True) 112 | sensor.set_auto_gain(True) # do some calibration at the start 113 | sensor.set_auto_whitebal(True) 114 | sensor.skip_frames(60) # Let new settings take effect. 115 | sensor.set_auto_gain(False) # now turn off autocalibration before we start color tracking 116 | sensor.set_auto_whitebal(False) 117 | 118 | 119 | 120 | while(True): 121 | clock.tick() # Track elapsed milliseconds between snapshots(). 122 | switch = switch_pin.value() # get value, 0 or 1 123 | if switch == 1 or not RC_control: # Teensy says you're in MV mode 124 | img = sensor.snapshot().lens_corr(strength = 1.8, zoom = 1) # do lens correcton for fisheye 125 | # img = img.rotation_corr(corners = TARGET_POINTS) # correct perspective to give top-down view 126 | line = img.get_regression([THRESHOLD], robust = False) # do linear regression for desired color 127 | if (line): 128 | img.draw_line(line.line(), color = 127) 129 | deflection_angle = -math.atan2(line.line()[1]-line.line()[3],line.line()[0]-line.line()[2]) 130 | 131 | # Convert angle in radians to degrees. 132 | deflection_angle = math.degrees(deflection_angle) 133 | deflection_angle = 90 - deflection_angle 134 | 135 | print("FPS %f" % clock.fps()) 136 | # print(deflection_angle) 137 | 138 | # Now you have an angle telling you how much to turn the robot which 139 | # incorporates the part of the line nearest to the robot and parts of 140 | # the line farther away from the robot for a better prediction. 141 | # print("Turn Angle: %f" % deflection_angle) 142 | now = pyb.millis() 143 | if now > old_time + 1.0 : # time has passed since last measurement 144 | if now > led_time + 1000: # switch LED every second 145 | if led_state == True: 146 | led_control(0) # turn off LED 147 | led_state = False 148 | else: 149 | led_control(2) # turn on Green LED 150 | led_state = True 151 | led_time = now # reset time counter 152 | 153 | measured_angle = deflection_angle + 90 154 | steer_angle = update_pid() 155 | old_time = now 156 | steer (steer_angle*-gain) 157 | # print(steer_angle*-gain) 158 | else: 159 | now = pyb.millis() 160 | if now > old_time + 1.0 : # time has passed since last measurement 161 | if now > led_time + 1000: # switch LED every second 162 | print("Under RC control") 163 | if led_state == True: 164 | led_control(0) # turn off LED 165 | led_state = False 166 | else: 167 | led_control(4) # turn on Blue LED 168 | led_state = True 169 | led_time = now # reset time counter 170 | 171 | -------------------------------------------------------------------------------- /colorlinefollowing.py: -------------------------------------------------------------------------------- 1 | # Color Line Following Example with PID Steering 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import Servo 10 | from pyb import LED 11 | from pyb import UART 12 | uart = UART(3, 9600) # no need to go faster than this. Slower = solid comms 13 | 14 | # Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max) 15 | # The below thresholds track in general red/green things. You may wish to tune them... 16 | # old thresholds = [(30, 100, 15, 127, 15, 127), # generic_red_thresholds 17 | # (30, 100, -64, -8, -32, 32), # generic_green_thresholds 18 | # (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds 19 | 20 | threshold_index = 3 21 | # 0 for red, 1 for green, 2 for blue 22 | 23 | thresholds = [(30, 100, 15, 127, 15, 127), # generic_red_thresholds 24 | (0, 83, -128, 15, -128, 127), # generic_green_thresholds 25 | (0, 100, -128, -10, -128, 51), # generic_blue_thresholds 26 | (0, 100, -47, 127, 14, 127)] # generic yellow threshold 27 | # You may pass up to 16 thresholds above. However, it's not really possible to segment any 28 | # scene with 16 thresholds before color thresholds start to overlap heavily. 29 | 30 | 31 | cruise_speed = 0 # how fast should the car drive, range from 1000 to 2000 32 | steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction 33 | steering_gain = 1.1 # calibration for your car's steering sensitivity 34 | steering_center = 80 # set to your car servo's center point 35 | kp = 0.4 # P term of the PID 36 | ki = 0.0 # I term of the PID 37 | kd = 0.3 # D term of the PID 38 | 39 | red_led = LED(1) 40 | green_led = LED(2) 41 | blue_led = LED(3) 42 | ir_led = LED(4) 43 | 44 | old_error = 0 45 | measured_angle = 0 46 | set_angle = 90 # this is the desired steering angle (straight ahead) 47 | p_term = 0 48 | i_term = 0 49 | d_term = 0 50 | old_time = pyb.millis() 51 | 52 | def led_control(x): 53 | if (x&1)==0: red_led.off() 54 | elif (x&1)==1: red_led.on() 55 | if (x&2)==0: green_led.off() 56 | elif (x&2)==2: green_led.on() 57 | if (x&4)==0: blue_led.off() 58 | elif (x&4)==4: blue_led.on() 59 | if (x&8)==0: ir_led.off() 60 | elif (x&8)==8: ir_led.on() 61 | 62 | def constrain(value, min, max): 63 | if value < min : 64 | return min 65 | if value > max : 66 | return max 67 | else: 68 | return value 69 | 70 | def steer(angle): 71 | global steering_gain, cruise_speed, steering_center 72 | angle = int(round((angle+steering_center)*steering_gain)) 73 | constrain(angle, 0, 180) 74 | ch1 = str(angle) # must convert to text to send via Serial 75 | ch2 = str(cruise_speed) # send throttle data, too 76 | print(angle) 77 | uart.write(ch1) # send to the Arduino. It looks for channel outputs in order, seperated by a "," and ended with a "\n" 78 | uart.write(",") 79 | uart.write(ch2) 80 | uart.write("\n") 81 | 82 | def update_pid(): 83 | global old_time, old_error, measured_angle, set_angle 84 | global p_term, i_term, d_term 85 | now = pyb.millis() 86 | dt = now - old_time 87 | error = set_angle - measured_angle 88 | de = error - old_error 89 | 90 | p_term = kp * error 91 | i_term += ki * error 92 | i_term = constrain(i_term, 0, 100) 93 | d_term = (de / dt) * kd 94 | 95 | old_error = error 96 | output = steering_direction * (p_term + i_term + d_term) 97 | output = constrain(output, -50, 50) 98 | return output 99 | 100 | 101 | # Each roi is (x, y, w, h). The line detection algorithm will try to find the 102 | # centroid of the largest blob in each roi. The x position of the centroids 103 | # will then be averaged with different weights where the most weight is assigned 104 | # to the roi near the bottom of the image and less to the next roi and so on. 105 | ROIS = [ # [ROI, weight] 106 | (0, 100, 160, 20, 0.1), # You'll need to tweak the weights for your app 107 | (0, 050, 160, 20, 0.3), # depending on how your robot is setup. 108 | (0, 000, 160, 20, 0.7) 109 | ] 110 | 111 | 112 | # Compute the weight divisor (we're computing this so you don't have to make weights add to 1). 113 | weight_sum = 0 114 | for r in ROIS: weight_sum += r[4] # r[4] is the roi weight. 115 | 116 | # Camera setup... 117 | clock = time.clock() # Tracks FPS. 118 | sensor.reset() # Initialize the camera sensor. 119 | sensor.__write_reg(0x6B, 0x22) # switches camera into advanced calibration mode. See this for more: http://forums.openmv.io/viewtopic.php?p=1358#p1358 120 | sensor.set_pixformat(sensor.RGB565) 121 | sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed. 122 | sensor.set_auto_gain(True) # do some calibration at the start 123 | sensor.set_auto_whitebal(True) 124 | sensor.skip_frames(60) # Let new settings take effect. 125 | sensor.set_auto_gain(False) # now turn off autocalibration before we start color tracking 126 | sensor.set_auto_whitebal(False) 127 | 128 | 129 | 130 | while(True): 131 | clock.tick() # Track elapsed milliseconds between snapshots(). 132 | img = sensor.snapshot() # Take a picture and return the image. 133 | centroid_sum = 0 134 | for r in ROIS: 135 | blobs = img.find_blobs([thresholds[threshold_index]], roi=r[0:4], merge=True) # r[0:4] is roi tuple. 136 | if blobs: 137 | # Find the index of the blob with the most pixels. 138 | most_pixels = 0 139 | largest_blob = 0 140 | for i in range(len(blobs)): 141 | if blobs[i].pixels() > most_pixels: 142 | most_pixels = blobs[i].pixels() 143 | largest_blob = i 144 | 145 | # Draw a rect around the blob. 146 | img.draw_rectangle(blobs[largest_blob].rect()) 147 | img.draw_cross(blobs[largest_blob].cx(), 148 | blobs[largest_blob].cy()) 149 | 150 | centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight. 151 | 152 | center_pos = (centroid_sum / weight_sum) # Determine center of line. 153 | 154 | # Convert the center_pos to a deflection angle. We're using a non-linear 155 | # operation so that the response gets stronger the farther off the line we 156 | # are. Non-linear operations are good to use on the output of algorithms 157 | # like this to cause a response "trigger". 158 | deflection_angle = 0 159 | # The 80 is from half the X res, the 60 is from half the Y res. The 160 | # equation below is just computing the angle of a triangle where the 161 | # opposite side of the triangle is the deviation of the center position 162 | # from the center and the adjacent side is half the Y res. This limits 163 | # the angle output to around -45 to 45. (It's not quite -45 and 45). 164 | deflection_angle = -math.atan((center_pos-80)/60) 165 | 166 | # Convert angle in radians to degrees. 167 | deflection_angle = math.degrees(deflection_angle) 168 | 169 | # Now you have an angle telling you how much to turn the robot by which 170 | # incorporates the part of the line nearest to the robot and parts of 171 | # the line farther away from the robot for a better prediction. 172 | # print("Turn Angle: %f" % deflection_angle) 173 | now = pyb.millis() 174 | if now > old_time + 1.0 : # time has passed since last measurement 175 | measured_angle = deflection_angle + 90 176 | steer_angle = update_pid() 177 | old_time = now 178 | steer (steer_angle) 179 | print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle)) 180 | -------------------------------------------------------------------------------- /colorlinefollowingRC.py: -------------------------------------------------------------------------------- 1 | # Color Line Following Example with PID Steering 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import Servo 10 | from pyb import LED 11 | from pyb import UART 12 | uart = UART(3, 19200) # Use UART 3 (pins 4 and 5). No need to go faster than this. Slower = solid comms 13 | 14 | # Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max) 15 | # The below thresholds track in general red/green things. You may wish to tune them... 16 | # old thresholds = [(30, 100, 15, 127, 15, 127), # generic_red_thresholds 17 | # (30, 100, -64, -8, -32, 32), # generic_green_thresholds 18 | # (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds 19 | 20 | threshold_index = 0 21 | # 0 for red, 1 for green, 2 for blue 22 | 23 | thresholds = [(0, 100, 17, 127, -76, 85), # generic_red_thresholds 24 | (0, 83, -128, 15, -128, 127), # generic_green_thresholds 25 | (0, 100, -128, -10, -128, 51)] # generic_blue_thresholds 26 | # You may pass up to 16 thresholds above. However, it's not really possible to segment any 27 | # scene with 16 thresholds before color thresholds start to overlap heavily. 28 | 29 | 30 | cruise_speed = 0 # how fast should the car drive, range from 1000 to 2000 31 | steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction 32 | steering_gain = 1.1 # calibration for your car's steering sensitivity 33 | steering_center = 80 # set to your car servo's center point 34 | kp = 0.4 # P term of the PID 35 | ki = 0.0 # I term of the PID 36 | kd = 0.3 # D term of the PID 37 | 38 | red_led = LED(1) 39 | green_led = LED(2) 40 | blue_led = LED(3) 41 | ir_led = LED(4) 42 | led_state = True 43 | led_time = pyb.millis() 44 | 45 | old_error = 0 46 | measured_angle = 0 47 | set_angle = 90 # this is the desired steering angle (straight ahead) 48 | p_term = 0 49 | i_term = 0 50 | d_term = 0 51 | old_time = pyb.millis() 52 | 53 | def led_control(x): 54 | if (x&1)==0: red_led.off() 55 | elif (x&1)==1: red_led.on() 56 | if (x&2)==0: green_led.off() 57 | elif (x&2)==2: green_led.on() 58 | if (x&4)==0: blue_led.off() 59 | elif (x&4)==4: blue_led.on() 60 | if (x&8)==0: ir_led.off() 61 | elif (x&8)==8: ir_led.on() 62 | 63 | def constrain(value, min, max): 64 | if value < min : 65 | return min 66 | if value > max : 67 | return max 68 | else: 69 | return value 70 | 71 | def steer(angle): 72 | global steering_gain, cruise_speed, steering_center 73 | angle = int(round((angle+steering_center)*steering_gain)) 74 | constrain(angle, 0, 180) 75 | ch1 = str(angle) # must convert to text to send via Serial 76 | ch2 = str(cruise_speed) # send throttle data, too 77 | print(angle) 78 | uart.write(ch1) # send to the Arduino. It looks for channel outputs in order, seperated by a "," and ended with a "\n" 79 | uart.write(",") 80 | uart.write(ch2) 81 | uart.write("\n") 82 | 83 | def update_pid(): 84 | global old_time, old_error, measured_angle, set_angle 85 | global p_term, i_term, d_term 86 | now = pyb.millis() 87 | dt = now - old_time 88 | error = set_angle - measured_angle 89 | de = error - old_error 90 | 91 | p_term = kp * error 92 | i_term += ki * error 93 | i_term = constrain(i_term, 0, 100) 94 | d_term = (de / dt) * kd 95 | 96 | old_error = error 97 | output = steering_direction * (p_term + i_term + d_term) 98 | output = constrain(output, -50, 50) 99 | return output 100 | 101 | 102 | # Each roi is (x, y, w, h). The line detection algorithm will try to find the 103 | # centroid of the largest blob in each roi. The x position of the centroids 104 | # will then be averaged with different weights where the most weight is assigned 105 | # to the roi near the bottom of the image and less to the next roi and so on. 106 | ROIS = [ # [ROI, weight] 107 | (102, 59, 139, 60, 0.1), # You'll need to tweak the weights for your app 108 | (83, 122, 181, 40, 0.3), # depending on how your robot is setup. 109 | (49, 174, 238, 49, 0.5) 110 | ] 111 | 112 | 113 | # Compute the weight divisor (we're computing this so you don't have to make weights add to 1). 114 | weight_sum = 0 115 | for r in ROIS: weight_sum += r[4] # r[4] is the roi weight. 116 | 117 | # Camera setup... 118 | clock = time.clock() # Tracks FPS. 119 | sensor.reset() # Initialize the camera sensor. 120 | sensor.__write_reg(0x6B, 0x22) # switches camera into advanced calibration mode. See this for more: http://forums.openmv.io/viewtopic.php?p=1358#p1358 121 | sensor.set_pixformat(sensor.RGB565) 122 | sensor.set_framesize(sensor.QVGA) # use QVGA for speed. 123 | sensor.set_vflip(True) 124 | sensor.set_auto_gain(True) # do some calibration at the start 125 | sensor.set_auto_whitebal(True) 126 | sensor.skip_frames(60) # Let new settings take effect. 127 | sensor.set_auto_gain(False) # now turn off autocalibration before we start color tracking 128 | sensor.set_auto_whitebal(False) 129 | 130 | 131 | 132 | while(True): 133 | clock.tick() # Track elapsed milliseconds between snapshots(). 134 | img = sensor.snapshot() # Take a picture and return the image. 135 | centroid_sum = 0 136 | for r in ROIS: 137 | blobs = img.find_blobs([thresholds[threshold_index]], roi=r[0:4], merge=True) # r[0:4] is roi tuple. 138 | if blobs: 139 | # Find the index of the blob with the most pixels. 140 | most_pixels = 0 141 | largest_blob = 0 142 | for i in range(len(blobs)): 143 | if blobs[i].pixels() > most_pixels: 144 | most_pixels = blobs[i].pixels() 145 | largest_blob = i 146 | 147 | # Draw a rect around the blob. 148 | img.draw_rectangle(blobs[largest_blob].rect()) 149 | img.draw_cross(blobs[largest_blob].cx(), 150 | blobs[largest_blob].cy()) 151 | 152 | centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight. 153 | 154 | center_pos = (centroid_sum / weight_sum) # Determine center of line. 155 | 156 | # Convert the center_pos to a deflection angle. We're using a non-linear 157 | # operation so that the response gets stronger the farther off the line we 158 | # are. Non-linear operations are good to use on the output of algorithms 159 | # like this to cause a response "trigger". 160 | deflection_angle = 0 161 | # The 80 is from half the X res, the 60 is from half the Y res. The 162 | # equation below is just computing the angle of a triangle where the 163 | # opposite side of the triangle is the deviation of the center position 164 | # from the center and the adjacent side is half the Y res. This limits 165 | # the angle output to around -45 to 45. (It's not quite -45 and 45). 166 | deflection_angle = -math.atan((center_pos-80)/60) 167 | 168 | # Convert angle in radians to degrees. 169 | deflection_angle = math.degrees(deflection_angle) 170 | 171 | # Now you have an angle telling you how much to turn the robot which 172 | # incorporates the part of the line nearest to the robot and parts of 173 | # the line farther away from the robot for a better prediction. 174 | # print("Turn Angle: %f" % deflection_angle) 175 | now = pyb.millis() 176 | if now > old_time + 1.0 : # time has passed since last measurement 177 | if now > led_time + 1000: # switch LED every second 178 | if led_state == True: 179 | led_control(0) # turn off LED 180 | led_state = False 181 | else: 182 | led_control(2) # turn on LED 183 | led_state = True 184 | led_time = now # reset time counter 185 | 186 | measured_angle = deflection_angle + 90 187 | steer_angle = update_pid() 188 | old_time = now 189 | steer (steer_angle) 190 | print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle)) 191 | -------------------------------------------------------------------------------- /linefollowing.py: -------------------------------------------------------------------------------- 1 | # This file is part of the OpenMV project. 2 | # Copyright (c) 2013-2017 Ibrahim Abdelkader & Kwabena W. Agyeman 3 | # Modified to use OpenMV Motor Shield by Chris Anderson, DIY Robocars 4 | # This work is licensed under the MIT license, see the file LICENSE for details. 5 | 6 | import sensor, image, time, math, pyb 7 | 8 | 9 | ########### 10 | # Settings 11 | ########### 12 | 13 | COLOR_LINE_FOLLOWING = True # False to use grayscale thresholds, true to use color thresholds. 14 | COLOR_THRESHOLDS = [( 94, 100, -27, 1, 20, 127)] # Yellow Line. 15 | GRAYSCALE_THRESHOLDS = [(240, 255)] # White Line. 16 | COLOR_HIGH_LIGHT_THRESHOLDS = [(80, 100, -10, 10, -10, 10)] 17 | GRAYSCALE_HIGH_LIGHT_THRESHOLDS = [(250, 255)] 18 | BINARY_VIEW = False # Helps debugging but costs FPS if on. 19 | DO_NOTHING = False # Just capture frames... 20 | FRAME_SIZE = sensor.QQVGA # Frame size. 21 | FRAME_REGION = 0.75 # Percentage of the image from the bottom (0 - 1.0). 22 | FRAME_WIDE = 1.0 # Percentage of the frame width. 23 | 24 | AREA_THRESHOLD = 0 # Raise to filter out false detections. 25 | PIXELS_THRESHOLD = 40 # Raise to filter out false detections. 26 | MAG_THRESHOLD = 4 # Raise to filter out false detections. 27 | MIXING_RATE = 0.9 # Percentage of a new line detection to mix into current steering. 28 | 29 | # Tweak these values for your robocar. 30 | THROTTLE_CUT_OFF_ANGLE = 1.0 # Maximum angular distance from 90 before we cut speed [0.0-90.0). 31 | THROTTLE_CUT_OFF_RATE = 0.5 # How much to cut our speed boost (below) once the above is passed (0.0-1.0]. 32 | THROTTLE_GAIN = 0.0 # e.g. how much to speed up on a straight away 33 | THROTTLE_OFFSET = 30.0 # e.g. default speed (0 to 100) 34 | THROTTLE_P_GAIN = 1.0 35 | THROTTLE_I_GAIN = 0.0 36 | THROTTLE_I_MIN = -0.0 37 | THROTTLE_I_MAX = 0.0 38 | THROTTLE_D_GAIN = 0.0 39 | 40 | # Tweak these values for your robocar. 41 | STEERING_OFFSET = 90 # Change this if you need to fix an imbalance in your car (0 to 180). 42 | STEERING_P_GAIN = -15.0 # Make this smaller as you increase your speed and vice versa. 43 | STEERING_I_GAIN = 0.0 44 | STEERING_I_MIN = -0.0 45 | STEERING_I_MAX = 0.0 46 | STEERING_D_GAIN = -12 # Make this larger as you increase your speed and vice versa. 47 | 48 | # Selects motor/servo controller method... 49 | ARDUINO_SERVO_CONTROLLER = False 50 | NATIVE_SERVO_CONTROLLER = False 51 | NATIVE_MOTOR_CONTROLLER = True 52 | 53 | # Tweak these values for your robocar if you're using servos. 54 | THROTTLE_SERVO_MIN_US = 1500 55 | THROTTLE_SERVO_MAX_US = 2000 56 | 57 | # Tweak these values for your robocar. 58 | STEERING_SERVO_MIN_US = 700 59 | STEERING_SERVO_MAX_US = 2300 60 | 61 | ########### 62 | # Setup 63 | ########### 64 | 65 | FRAME_REGION = max(min(FRAME_REGION, 1.0), 0.0) 66 | FRAME_WIDE = max(min(FRAME_WIDE, 1.0), 0.0) 67 | MIXING_RATE = max(min(MIXING_RATE, 1.0), 0.0) 68 | 69 | THROTTLE_CUT_OFF_ANGLE = max(min(THROTTLE_CUT_OFF_ANGLE, 89.99), 0) 70 | THROTTLE_CUT_OFF_RATE = max(min(THROTTLE_CUT_OFF_RATE, 1.0), 0.01) 71 | 72 | THROTTLE_OFFSET = max(min(THROTTLE_OFFSET, 100), 0) 73 | STEERING_OFFSET = max(min(STEERING_OFFSET, 180), 0) 74 | 75 | # Handle if these were reversed... 76 | tmp = max(THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US) 77 | THROTTLE_SERVO_MIN_US = min(THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US) 78 | THROTTLE_SERVO_MAX_US = tmp 79 | 80 | # Handle if these were reversed... 81 | tmp = max(STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US) 82 | STEERING_SERVO_MIN_US = min(STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US) 83 | STEERING_SERVO_MAX_US = tmp 84 | 85 | device = None 86 | 87 | if ARDUINO_SERVO_CONTROLLER: 88 | device = pyb.UART(3, 19200, timeout_char = 1000) 89 | 90 | if NATIVE_SERVO_CONTROLLER: 91 | import servo 92 | import machine 93 | device = servo.Servos(machine.I2C(sda = machine.Pin("P5"), scl = machine.Pin("P4")), address = 0x40, freq = 50) 94 | 95 | if NATIVE_MOTOR_CONTROLLER: 96 | from pyb import Pin, Timer 97 | 98 | # these are motor driver pins, which set the direction of each motor 99 | pinADir0 = pyb.Pin('P0', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE) 100 | pinADir1 = pyb.Pin('P1', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE) 101 | pinBDir0 = pyb.Pin('P2', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE) 102 | pinBDir1 = pyb.Pin('P3', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE) 103 | 104 | # Dir0/1 must be not equal to each other for forward or backwards 105 | # operation. If they are equal then that's a brake operation. 106 | # If they are not equal then the motor will spin one way other the 107 | # other depending on its hookup and the value of channel 0. 108 | 109 | pinBDir0.value(0) 110 | pinBDir1.value(1) 111 | 112 | tim = Timer(4, freq=1000) # Frequency in Hz 113 | ch1 = tim.channel(1, pyb.Timer.PWM, pin=pyb.Pin("P7")) 114 | ch2 = tim.channel(2, pyb.Timer.PWM, pin=pyb.Pin("P8")) 115 | radians_degrees = 57.3 # constant to convert from radians to degrees 116 | 117 | cruise_speed = 50 118 | steering_direction = 1 # use this to reverse the steering if your car goes in the wrong direction 119 | steering_gain = 1.0 # calibration for your car's steering sensitivity 120 | steering_center = 0 # set to your car's steering center point 121 | 122 | def constrain(value, min, max): 123 | if value < min : 124 | return min 125 | if value > max : 126 | return max 127 | else: 128 | return value 129 | 130 | def steer(throttle, angle): 131 | global steering_gain, cruise_speed, steering_center 132 | angle = int(round(angle+steering_center)) 133 | angle = constrain(angle, 0, 180) 134 | angle = angle - 90 135 | angle = radians_degrees * math.tan(angle/radians_degrees) # take the tangent to create a non-linear response curve 136 | angle = angle * steering_gain 137 | print ("Calculated angle", angle) 138 | left = angle*steering_direction + throttle + cruise_speed 139 | left = constrain (left, 0, 100) 140 | right = -1*angle*steering_direction + throttle + cruise_speed 141 | right = constrain (right, 0, 100) 142 | print ("left: ", left) 143 | print ("right: ", right) 144 | # Generate a 1KHz square wave on TIM4 with each channel 145 | ch1.pulse_width_percent(left) 146 | ch2.pulse_width_percent(right) 147 | 148 | 149 | # This function maps the output of the linear regression function to a driving vector for steering 150 | # the robocar. See https://openmv.io/blogs/news/linear-regression-line-following for more info. 151 | 152 | old_cx_normal = None 153 | def figure_out_my_steering(line, img): 154 | global old_cx_normal 155 | 156 | # Rho is computed using the inverse of this code below in the actual OpenMV Cam code. 157 | # This formula comes from the Hough line detection formula (see the wikipedia page for more). 158 | # Anyway, the output of this calculations below are a point centered vertically in the middle 159 | # of the image and to the left or right such that the line goes through it (cx may be off the image). 160 | cy = img.height() / 2 161 | cx = (line.rho() - (cy * math.sin(math.radians(line.theta())))) / math.cos(math.radians(line.theta())) 162 | 163 | # "cx_middle" is now the distance from the center of the line. This is our error method to stay 164 | # on the line. "cx_normal" normalizes the error to something like -1/+1 (it will go over this). 165 | cx_middle = cx - (img.width() / 2) 166 | cx_normal = cx_middle / (img.width() / 2) 167 | # Note that "cx_normal" may be larger than -1/+1. When the value is between -1/+1 this means the 168 | # robot is driving basically straight and needs to only turn lightly left or right. When the value 169 | # is outside -1/+1 it means you need to turn VERY hard to the left or right to get back on the 170 | # line. This maps to the case of the robot driving into a horizontal line. "cx_normal" will 171 | # then approach -inf/+inf depending on how horizontal the line is. What's nice is that this 172 | # is exactly the behavior we want and it gets up back on the line! 173 | 174 | if old_cx_normal != None: old_cx_normal = (cx_normal * MIXING_RATE) + (old_cx_normal * (1.0 - MIXING_RATE)) 175 | else: old_cx_normal = cx_normal 176 | return old_cx_normal 177 | 178 | # Solve: THROTTLE_CUT_OFF_RATE = pow(sin(90 +/- THROTTLE_CUT_OFF_ANGLE), x) for x... 179 | # -> sin(90 +/- THROTTLE_CUT_OFF_ANGLE) = cos(THROTTLE_CUT_OFF_ANGLE) 180 | t_power = math.log(THROTTLE_CUT_OFF_RATE) / math.log(math.cos(math.radians(THROTTLE_CUT_OFF_ANGLE))) 181 | 182 | def figure_out_my_throttle(steering): # steering -> [0:180] 183 | 184 | # pow(sin()) of the steering angle is only non-zero when driving straight... e.g. steering ~= 90 185 | t_result = math.pow(math.sin(math.radians(max(min(steering, 179.99), 0.0))), t_power) 186 | 187 | return (t_result * THROTTLE_GAIN) + THROTTLE_OFFSET 188 | 189 | # 190 | # Servo Control Code 191 | # 192 | 193 | 194 | 195 | # throttle [0:100] (101 values) -> [THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US] 196 | # steering [0:180] (181 values) -> [STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US] 197 | def set_servos(throttle, steering): 198 | if NATIVE_MOTOR_CONTROLLER: 199 | steer(throttle, steering) 200 | if ARDUINO_SERVO_CONTROLLER: 201 | throttle = THROTTLE_SERVO_MIN_US + ((throttle * (THROTTLE_SERVO_MAX_US - THROTTLE_SERVO_MIN_US + 1)) / 101) 202 | steering = STEERING_SERVO_MIN_US + ((steering * (STEERING_SERVO_MAX_US - STEERING_SERVO_MIN_US + 1)) / 181) 203 | device.write("{%05d,%05d}\r\n" % (throttle, steering)) 204 | if NATIVE_SERVO_CONTROLLER: 205 | throttle = THROTTLE_SERVO_MIN_US + ((throttle * (THROTTLE_SERVO_MAX_US - THROTTLE_SERVO_MIN_US + 1)) / 101) 206 | steering = STEERING_SERVO_MIN_US + ((steering * (STEERING_SERVO_MAX_US - STEERING_SERVO_MIN_US + 1)) / 181) 207 | device.position(0, us=throttle) 208 | device.position(1, us=steering) 209 | 210 | # 211 | # Camera Control Code 212 | # 213 | 214 | sensor.reset() 215 | sensor.set_pixformat(sensor.RGB565 if COLOR_LINE_FOLLOWING else sensor.GRAYSCALE) 216 | sensor.set_framesize(FRAME_SIZE) 217 | sensor.set_vflip(True) 218 | sensor.set_hmirror(True) 219 | sensor.set_windowing((int((sensor.width() / 2) - ((sensor.width() / 2) * FRAME_WIDE)), int(sensor.height() * (1.0 - FRAME_REGION)), \ 220 | int((sensor.width() / 2) + ((sensor.width() / 2) * FRAME_WIDE)), int(sensor.height() * FRAME_REGION))) 221 | sensor.skip_frames(time = 200) 222 | if COLOR_LINE_FOLLOWING: sensor.set_auto_gain(False) 223 | if COLOR_LINE_FOLLOWING: sensor.set_auto_whitebal(False) 224 | clock = time.clock() 225 | #sensor.set_auto_exposure(False, \ 226 | # exposure_us = 300) 227 | 228 | 229 | ########### 230 | # Loop 231 | ########### 232 | 233 | old_time = pyb.millis() 234 | 235 | throttle_old_result = None 236 | throttle_i_output = 0 237 | throttle_output = THROTTLE_OFFSET 238 | 239 | steering_old_result = None 240 | steering_i_output = 0 241 | steering_output = STEERING_OFFSET 242 | 243 | while True: 244 | clock.tick() 245 | img = sensor.snapshot() 246 | img.binary(COLOR_HIGH_LIGHT_THRESHOLDS if COLOR_LINE_FOLLOWING else GRAYSCALE_HIGH_LIGHT_THRESHOLDS, zero = True) 247 | img.histeq() 248 | 249 | if BINARY_VIEW: img = img.binary(COLOR_THRESHOLDS if COLOR_LINE_FOLLOWING else GRAYSCALE_THRESHOLDS) 250 | if BINARY_VIEW: img.erode(1, threshold = 5).dilate(1, threshold = 1) 251 | if DO_NOTHING: continue 252 | 253 | # We call get regression below to get a robust linear regression of the field of view. 254 | # This returns a line object which we can use to steer the robocar. 255 | line = img.get_regression(([(50, 100, -128, 127, -128, 127)] if BINARY_VIEW else COLOR_THRESHOLDS) if COLOR_LINE_FOLLOWING \ 256 | else ([(127, 255)] if BINARY_VIEW else GRAYSCALE_THRESHOLDS), \ 257 | area_threshold = AREA_THRESHOLD, pixels_threshold = PIXELS_THRESHOLD, \ 258 | robust = True) 259 | 260 | print_string = "" 261 | if line and (line.magnitude() >= MAG_THRESHOLD): 262 | img.draw_line(line.line(), color = (127, 127, 127) if COLOR_LINE_FOLLOWING else 127) 263 | 264 | new_time = pyb.millis() 265 | delta_time = new_time - old_time 266 | old_time = new_time 267 | 268 | # 269 | # Figure out steering and do steering PID 270 | # 271 | 272 | steering_new_result = figure_out_my_steering(line, img) 273 | steering_delta_result = (steering_new_result - steering_old_result) if (steering_old_result != None) else 0 274 | steering_old_result = steering_new_result 275 | 276 | steering_p_output = steering_new_result # Standard PID Stuff here... nothing particularly interesting :) 277 | steering_i_output = max(min(steering_i_output + steering_new_result, STEERING_I_MAX), STEERING_I_MIN) 278 | steering_d_output = ((steering_delta_result * 1000) / delta_time) if delta_time else 0 279 | steering_pid_output = (STEERING_P_GAIN * steering_p_output) + \ 280 | (STEERING_I_GAIN * steering_i_output) + \ 281 | (STEERING_D_GAIN * steering_d_output) 282 | 283 | # Steering goes from [-90,90] but we need to output [0,180] for the servos. 284 | steering_output = STEERING_OFFSET + max(min(round(steering_pid_output), 180 - STEERING_OFFSET), STEERING_OFFSET - 180) 285 | 286 | # 287 | # Figure out throttle and do throttle PID 288 | # 289 | 290 | throttle_new_result = figure_out_my_throttle(steering_output) 291 | throttle_delta_result = (throttle_new_result - throttle_old_result) if (throttle_old_result != None) else 0 292 | throttle_old_result = throttle_new_result 293 | 294 | throttle_p_output = throttle_new_result # Standard PID Stuff here... nothing particularly interesting :) 295 | throttle_i_output = max(min(throttle_i_output + throttle_new_result, THROTTLE_I_MAX), THROTTLE_I_MIN) 296 | throttle_d_output = ((throttle_delta_result * 1000) / delta_time) if delta_time else 0 297 | throttle_pid_output = (THROTTLE_P_GAIN * throttle_p_output) + \ 298 | (THROTTLE_I_GAIN * throttle_i_output) + \ 299 | (THROTTLE_D_GAIN * throttle_d_output) 300 | 301 | # Throttle goes from 0% to 100%. 302 | throttle_output = max(min(round(throttle_pid_output), 100), 0) 303 | 304 | print_string = "Line Ok - throttle %d, steering %d - line t: %d, r: %d" % \ 305 | (throttle_output , steering_output, line.theta(), line.rho()) 306 | 307 | else: 308 | print_string = "Line Lost - throttle %d, steering %d" % (throttle_output , steering_output) 309 | 310 | set_servos(throttle_output, steering_output) 311 | print("FPS %f - %s" % (clock.fps(), print_string)) 312 | -------------------------------------------------------------------------------- /newcolor.py: -------------------------------------------------------------------------------- 1 | # Color Line Following Example with PID Steering 2 | 3 | # 4 | # For this script to work properly you should point the camera at a line at a 5 | # 45 or so degree angle. Please make sure that only the line is within the 6 | # camera's field of view. 7 | 8 | import sensor, image, pyb, math, time 9 | from pyb import Servo 10 | from pyb import LED 11 | from pyb import UART 12 | uart = UART(3, 9600) # no need to go faster than this. Slower = solid comms 13 | 14 | # Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max) 15 | 16 | 17 | threshold_index = 0 18 | # 0 for red, 1 for green, 2 for blue 19 | 20 | thresholds = [(0, 100, 21, 127, -128, 127), # generic_red_thresholds 21 | (0, 83, -128, 15, -128, 127), # generic_green_thresholds 22 | (0, 100, -128, -10, -128, 51), # generic_blue_thresholds 23 | (80, 100, -26, 127, -1, 127)] # generic yellow threshold 24 | # You may pass up to 16 thresholds above. However, it's not really possible to segment any 25 | # scene with 16 thresholds before color thresholds start to overlap heavily. 26 | 27 | 28 | cruise_speed = 0 # how fast should the car drive, range from 1000 to 2000 29 | steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction 30 | steering_gain = 1.3 # calibration for your car's steering sensitivity 31 | steering_center = 80 # set to your car servo's center point 32 | kp = 0.4 # P term of the PID 33 | ki = 0.0 # I term of the PID 34 | kd = 0.3 # D term of the PID 35 | 36 | red_led = LED(1) 37 | green_led = LED(2) 38 | blue_led = LED(3) 39 | ir_led = LED(4) 40 | 41 | old_error = 0 42 | measured_angle = 0 43 | set_angle = 90 # this is the desired steering angle (straight ahead) 44 | p_term = 0 45 | i_term = 0 46 | d_term = 0 47 | old_time = pyb.millis() 48 | 49 | def led_control(x): 50 | if (x&1)==0: red_led.off() 51 | elif (x&1)==1: red_led.on() 52 | if (x&2)==0: green_led.off() 53 | elif (x&2)==2: green_led.on() 54 | if (x&4)==0: blue_led.off() 55 | elif (x&4)==4: blue_led.on() 56 | if (x&8)==0: ir_led.off() 57 | elif (x&8)==8: ir_led.on() 58 | 59 | def constrain(value, min, max): 60 | if value < min : 61 | return min 62 | if value > max : 63 | return max 64 | else: 65 | return value 66 | 67 | def steer(angle): 68 | global steering_gain, cruise_speed, steering_center 69 | angle = int(round((angle+steering_center)*steering_gain)) 70 | constrain(angle, 0, 180) 71 | ch1 = str(angle) # must convert to text to send via Serial 72 | ch2 = str(cruise_speed) # send throttle data, too 73 | print(angle) 74 | uart.write(ch1) # send to the Arduino. It looks for channel outputs in order, seperated by a "," and ended with a "\n" 75 | uart.write(",") 76 | uart.write(ch2) 77 | uart.write("\n") 78 | 79 | def update_pid(): 80 | global old_time, old_error, measured_angle, set_angle 81 | global p_term, i_term, d_term 82 | now = pyb.millis() 83 | dt = now - old_time 84 | error = set_angle - measured_angle 85 | de = error - old_error 86 | 87 | p_term = kp * error 88 | i_term += ki * error 89 | i_term = constrain(i_term, 0, 100) 90 | d_term = (de / dt) * kd 91 | 92 | old_error = error 93 | output = steering_direction * (p_term + i_term + d_term) 94 | output = constrain(output, -50, 50) 95 | return output 96 | 97 | 98 | # Each roi is (x, y, w, h). The line detection algorithm will try to find the 99 | # centroid of the largest blob in each roi. The x position of the centroids 100 | # will then be averaged with different weights where the most weight is assigned 101 | # to the roi near the bottom of the image and less to the next roi and so on. 102 | ROIS = [ # [ROI, weight] 103 | (0, 100, 160, 20, 0.6), # You'll need to tweak the weights for your app 104 | (0, 050, 160, 20, 0.6), # depending on how your robot is setup. 105 | (0, 000, 160, 20, 0.0) 106 | ] 107 | 108 | 109 | # Compute the weight divisor (we're computing this so you don't have to make weights add to 1). 110 | weight_sum = 0 111 | for r in ROIS: weight_sum += r[4] # r[4] is the roi weight. 112 | 113 | # Camera setup... 114 | clock = time.clock() # Tracks FPS. 115 | sensor.reset() # Initialize the camera sensor. 116 | sensor.__write_reg(0x6B, 0x22) # switches camera into advanced calibration mode. See this for more: http://forums.openmv.io/viewtopic.php?p=1358#p1358 117 | sensor.set_pixformat(sensor.RGB565) 118 | sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed. 119 | sensor.set_auto_gain(True) # do some calibration at the start 120 | sensor.set_auto_whitebal(True) 121 | sensor.skip_frames(60) # Let new settings take effect. 122 | sensor.set_auto_gain(False) # now turn off autocalibration before we start color tracking 123 | sensor.set_auto_whitebal(False) 124 | 125 | 126 | 127 | while(True): 128 | clock.tick() # Track elapsed milliseconds between snapshots(). 129 | img = sensor.snapshot() # Take a picture and return the image. 130 | centroid_sum = 0 131 | for r in ROIS: 132 | blobs = img.find_blobs([thresholds[threshold_index]], roi=r[0:4], merge=True) # r[0:4] is roi tuple. 133 | if blobs: 134 | # Find the index of the blob with the most pixels. 135 | most_pixels = 0 136 | largest_blob = 0 137 | for i in range(len(blobs)): 138 | if blobs[i].pixels() > most_pixels: 139 | most_pixels = blobs[i].pixels() 140 | largest_blob = i 141 | 142 | # Draw a rect around the blob. 143 | img.draw_rectangle(blobs[largest_blob].rect()) 144 | img.draw_cross(blobs[largest_blob].cx(), 145 | blobs[largest_blob].cy()) 146 | 147 | centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight. 148 | 149 | center_pos = (centroid_sum / weight_sum) # Determine center of line. 150 | 151 | # Convert the center_pos to a deflection angle. We're using a non-linear 152 | # operation so that the response gets stronger the farther off the line we 153 | # are. Non-linear operations are good to use on the output of algorithms 154 | # like this to cause a response "trigger". 155 | deflection_angle = 0 156 | # The 80 is from half the X res, the 60 is from half the Y res. The 157 | # equation below is just computing the angle of a triangle where the 158 | # opposite side of the triangle is the deviation of the center position 159 | # from the center and the adjacent side is half the Y res. This limits 160 | # the angle output to around -45 to 45. (It's not quite -45 and 45). 161 | deflection_angle = -math.atan((center_pos-80)/60) 162 | 163 | # Convert angle in radians to degrees. 164 | deflection_angle = math.degrees(deflection_angle) 165 | 166 | # Now you have an angle telling you how much to turn the robot by which 167 | # incorporates the part of the line nearest to the robot and parts of 168 | # the line farther away from the robot for a better prediction. 169 | # print("Turn Angle: %f" % deflection_angle) 170 | now = pyb.millis() 171 | if now > old_time + 1.0 : # time has passed since last measurement 172 | measured_angle = deflection_angle + 90 173 | steer_angle = update_pid() 174 | old_time = now 175 | steer (steer_angle) 176 | print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle)) 177 | -------------------------------------------------------------------------------- /pidrover.py: -------------------------------------------------------------------------------- 1 | # Autonomous rover code by Chris Anderson, 3DR, DIY Robocars 2 | # Copyrigbt 2017 3 | # Released under a BSD 2.0 Open Source Licence 4 | 5 | import sensor, image, pyb, math, time 6 | from pyb import Servo 7 | from pyb import LED 8 | 9 | s1 = Servo(1) # P7 Motor 10 | s2 = Servo(2) # P8 Steering 11 | print (s1.calibration()) # show throttle servo calibration 12 | cruise_speed = 0 # how fast should the car drive, range from 1000 to 2000 13 | steering_gain = 500 14 | kp = 0.8 # P term of the PID 15 | ki = 0 # I term of the PID 16 | kd = 0 # D term of the PID 17 | 18 | red_led = LED(1) 19 | green_led = LED(2) 20 | blue_led = LED(3) 21 | ir_led = LED(4) 22 | 23 | old_error = 0 24 | measured_angle = 0 25 | set_angle = 90 # this is the desired steering angle (straight ahead) 26 | p_term = 0 27 | i_term = 0 28 | d_term = 0 29 | old_time = pyb.millis() 30 | 31 | def led_control(x): 32 | if (x&1)==0: red_led.off() 33 | elif (x&1)==1: red_led.on() 34 | if (x&2)==0: green_led.off() 35 | elif (x&2)==2: green_led.on() 36 | if (x&4)==0: blue_led.off() 37 | elif (x&4)==4: blue_led.on() 38 | if (x&8)==0: ir_led.off() 39 | elif (x&8)==8: ir_led.on() 40 | 41 | def constrain(value, min, max): 42 | if value < min : 43 | return min 44 | if value > max : 45 | return max 46 | else: 47 | return value 48 | 49 | def steer(angle): 50 | global steering_gain 51 | s1.angle(angle) # steer 52 | pyb.delay(10) 53 | 54 | def update_pid(): 55 | global old_time, old_error, measured_angle, set_angle 56 | global p_term, i_term, d_term 57 | now = pyb.millis() 58 | dt = now - old_time 59 | error = set_angle - measured_angle 60 | de = error - old_error 61 | 62 | p_term = kp * error 63 | i_term += ki * error 64 | i_term = constrain(i_term, 0, 100) 65 | d_term = (de / dt) * kd 66 | 67 | old_error = error 68 | output = p_term + i_term + d_term 69 | output = constrain(output, -50, 50) 70 | return output 71 | 72 | sensor.reset() # Initialize the camera sensor. 73 | sensor.set_pixformat(sensor.GRAYSCALE) # or sensor.RGB565 74 | sensor.set_framesize(sensor.QQVGA) # or sensor.QVGA (or others) 75 | clock = time.clock() # Tracks FPS. 76 | #s1.pulse_width(2000) # initialize the motor controller with a high signal for a second 77 | #pyb.delay(1000) 78 | #s1.pulse_width(1000) # return motor controller to low/off throttle 79 | while(True): 80 | led_control(2) 81 | # s1.speed(120) Start motor; currently disabled while I use RC for throttle 82 | # pyb.delay(10) 83 | clock.tick() # Track elapsed milliseconds between snapshots(). 84 | img = sensor.snapshot() # Take a picture and return the image. 85 | img.find_edges(image.EDGE_CANNY, threshold=(60, 150)) # Find edges 86 | lines = img.find_lines(threshold=35) # Find lines. 87 | counter = 0 88 | totalangle = 0 89 | for l in lines: 90 | img.draw_line(l, color=(127)) # Draw lines 91 | if lines: 92 | if (l[2]-l[0]) != 0: # don't allow vertical lines (undefined arctan) 93 | angle = -math.atan2(-(l[3]-l[1]),(l[2]-l[0])) # arctan (deltaY/deltaX); we use atan2 so we can tell what quadrant it's in 94 | angle = math.degrees(angle) # convert from radians to degrees 95 | if angle < 140 and angle > 40: # ignore lines that are mostly horizontal 96 | totalangle = totalangle + angle # add up all the line angles so we can average them 97 | counter = counter + 1 98 | if counter != 0: 99 | now = pyb.millis() 100 | if now > old_time + 1.0 : # time has passed since last measurement 101 | measured_angle = totalangle/counter # average of angles 102 | steer_angle = update_pid() 103 | old_time = now 104 | steer (steer_angle) 105 | print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle)) 106 | # s1.pulse_width(cruise_speed) # move forward at cruise speed; currently disabled while I run throttle with RC 107 | 108 | -------------------------------------------------------------------------------- /pulsein_test/pulsein_test.ino: -------------------------------------------------------------------------------- 1 | 2 | const int BAUD_RATE = 9600; 3 | int RC; 4 | int RC1_pin = 3; 5 | int RC2_pin = 4; 6 | int RC3_pin = 5; 7 | unsigned long RC1_value; 8 | unsigned long RC2_value; 9 | unsigned long RC3_value; 10 | 11 | 12 | void setup() { 13 | Serial.begin(BAUD_RATE); 14 | pinMode(RC1_pin, INPUT); 15 | pinMode(RC2_pin, INPUT); 16 | pinMode(RC3_pin, INPUT); 17 | } 18 | 19 | void loop() { 20 | // put your main code here, to run repeatedly: 21 | 22 | RC1_value = pulseIn(RC1_pin, HIGH); 23 | RC2_value = pulseIn(RC2_pin, HIGH); 24 | RC3_value = pulseIn(RC3_pin, HIGH); 25 | 26 | Serial.print(RC1_value); 27 | Serial.print(" "); 28 | Serial.print(RC2_value); 29 | Serial.print(" "); 30 | Serial.println(RC3_value); 31 | } 32 | -------------------------------------------------------------------------------- /roslaunch-duckiebot-11119.log: -------------------------------------------------------------------------------- 1 | [roslaunch][INFO] 2018-01-01 22:46:21,407: Checking log directory for disk usage. This may take awhile. 2 | Press Ctrl-C to interrupt 3 | [roslaunch][INFO] 2018-01-01 22:46:21,424: Done checking log file disk usage. Usage is <1GB. 4 | [roslaunch][INFO] 2018-01-01 22:46:21,426: roslaunch starting with args ['/opt/ros/kinetic/bin/roslaunch', 'duckietown', 'joystick.launch', 'veh:=chris'] 5 | [roslaunch][INFO] 2018-01-01 22:46:21,427: roslaunch env is {'ROS_LANG_DISABLE': 'gennodejs:geneus:genlisp', 'ROS_DISTRO': 'kinetic', 'LESSOPEN': '| /usr/bin/lesspipe %s', 'SSH_CLIENT': '192.168.86.42 9191 22', 'LOGNAME': 'pi', 'USER': 'pi', 'HOME': '/home/pi', 'ROS_HOSTNAME': 'duckiebot.local', 'PATH': '/opt/ros/kinetic/bin:/home/pi/bin:/home/pi/.local/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin', 'DUCKIEFLEET_ROOT': '/home/pi/duckiefleet', 'CMAKE_PREFIX_PATH': '/home/pi/Software/catkin_ws/devel:/opt/ros/kinetic', 'LD_LIBRARY_PATH': '/home/pi/Software/catkin_ws/devel/lib:/opt/ros/kinetic/lib', 'SSH_CONNECTION': '192.168.86.42 9191 192.168.86.20 22', 'LANG': 'en_US.UTF-8', 'TERM': 'cygwin', 'SHELL': '/bin/bash', 'SHLVL': '1', 'ROS_LOG_FILENAME': '/home/pi/.ros/log/346f880e-ef3d-11e7-8913-b827ebe4d5fb/roslaunch-duckiebot-11119.log', 'ROS_ETC_DIR': '/opt/ros/kinetic/etc/ros', 'XDG_RUNTIME_DIR': '/run/user/1001', 'PYTHONPATH': '/home/pi/Software/catkin_ws/devel/lib/python2.7/dist-packages:/opt/ros/kinetic/lib/python2.7/dist-packages:/home/pi/Software/catkin_ws/src:/home/pi/Software/catkin_ws/src', 'ROS_ROOT': '/opt/ros/kinetic/share/ros', 'PKG_CONFIG_PATH': '/home/pi/Software/catkin_ws/devel/lib/pkgconfig:/opt/ros/kinetic/lib/pkgconfig', 'ROS_PACKAGE_PATH': '/home/pi/Software/catkin_ws/src:/opt/ros/kinetic/share', 'XDG_SESSION_ID': 'c1', '_': '/opt/ros/kinetic/bin/roslaunch', 'DUCKIETOWN_ROOT': '/home/pi/Software', 'LESSCLOSE': '/usr/bin/lesspipe %s %s', 'GTK_MODULES': 'gail:atk-bridge', 'SSH_TTY': '/dev/pts/0', 'OLDPWD': '/usr', 'HOSTNAME': 'duckiebot', 'ROSLISP_PACKAGE_DIRECTORIES': '/home/pi/Software/catkin_ws/devel/share/common-lisp', 'XDG_DATA_DIRS': '/usr/local/share:/usr/share:/var/lib/snapd/desktop', 'PWD': '/home/pi', 'ROS_MASTER_URI': 'http://localhost:11311', 'MAIL': '/var/mail/pi', 'LS_COLORS': 'rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.Z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.jpg=01;35:*.jpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:'} 6 | [roslaunch][INFO] 2018-01-01 22:46:21,428: starting in server mode 7 | [roslaunch.parent][INFO] 2018-01-01 22:46:21,430: starting roslaunch parent run 8 | [roslaunch][INFO] 2018-01-01 22:46:21,432: loading roscore config file /opt/ros/kinetic/etc/ros/roscore.xml 9 | [roslaunch][INFO] 2018-01-01 22:46:22,463: Added core node of type [rosout/rosout] in namespace [/] 10 | [roslaunch.config][INFO] 2018-01-01 22:46:22,465: loading config file /home/pi/Software/catkin_ws/src/00-infrastructure/duckietown/launch/joystick.launch 11 | [roslaunch][INFO] 2018-01-01 22:46:23,636: Added node of type [joy/joy_node] in namespace [/chris/] 12 | [roslaunch][INFO] 2018-01-01 22:46:23,855: Added node of type [joy_mapper/joy_mapper_node.py] in namespace [/chris/] 13 | [roslaunch][INFO] 2018-01-01 22:46:23,875: Added node of type [dagu_car/inverse_kinematics_node.py] in namespace [/chris/] 14 | [roslaunch][INFO] 2018-01-01 22:46:23,897: Added node of type [dagu_car/forward_kinematics_node.py] in namespace [/chris/] 15 | [roslaunch][INFO] 2018-01-01 22:46:23,917: Added node of type [dagu_car/velocity_to_pose_node.py] in namespace [/chris/] 16 | [roslaunch][INFO] 2018-01-01 22:46:23,928: Added node of type [dagu_car/wheels_driver_node.py] in namespace [/chris/] 17 | [roslaunch][INFO] 2018-01-01 22:46:23,929: ... selected machine [chris] for node of type [joy/joy_node] 18 | [roslaunch][INFO] 2018-01-01 22:46:23,930: ... selected machine [chris] for node of type [joy_mapper/joy_mapper_node.py] 19 | [roslaunch][INFO] 2018-01-01 22:46:23,931: ... selected machine [chris] for node of type [dagu_car/inverse_kinematics_node.py] 20 | [roslaunch][INFO] 2018-01-01 22:46:23,932: ... selected machine [chris] for node of type [dagu_car/forward_kinematics_node.py] 21 | [roslaunch][INFO] 2018-01-01 22:46:23,933: ... selected machine [chris] for node of type [dagu_car/velocity_to_pose_node.py] 22 | [roslaunch][INFO] 2018-01-01 22:46:23,934: ... selected machine [chris] for node of type [dagu_car/wheels_driver_node.py] 23 | [roslaunch][ERROR] 2018-01-01 22:46:28,949: cannot resolve host address for machine [chris.local] 24 | [roslaunch][ERROR] 2018-01-01 22:46:28,950: The traceback for the exception was written to the log file 25 | [roslaunch][ERROR] 2018-01-01 22:46:28,955: Traceback (most recent call last): 26 | File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/__init__.py", line 306, in main 27 | p.start() 28 | File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/parent.py", line 268, in start 29 | self._start_infrastructure() 30 | File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/parent.py", line 217, in _start_infrastructure 31 | self._load_config() 32 | File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/parent.py", line 132, in _load_config 33 | roslaunch_strs=self.roslaunch_strs, verbose=self.verbose) 34 | File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/config.py", line 471, in load_config_default 35 | config.assign_machines() 36 | File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/config.py", line 222, in assign_machines 37 | if [m for m in machine_unify_dict.values() if not is_machine_local(m)]: 38 | File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/core.py", line 96, in is_machine_local 39 | raise RLException("cannot resolve host address for machine [%s]"%machine.address) 40 | RLException: cannot resolve host address for machine [chris.local] 41 | 42 | [rospy.core][INFO] 2018-01-01 22:46:28,957: signal_shutdown [atexit] 43 | -------------------------------------------------------------------------------- /rover.py: -------------------------------------------------------------------------------- 1 | # Autonomous rover code by Chris Anderson, 3DR, DIY Robocars 2 | # Copyrigbt 2017 3 | # Released under a BSD 2.0 Open Source Licence 4 | 5 | import sensor, image, time, pyb 6 | from pyb import Servo 7 | from pyb import LED 8 | 9 | red_led = LED(1) 10 | green_led = LED(2) 11 | blue_led = LED(3) 12 | ir_led = LED(4) 13 | 14 | def led_control(x): 15 | if (x&1)==0: red_led.off() 16 | elif (x&1)==1: red_led.on() 17 | if (x&2)==0: green_led.off() 18 | elif (x&2)==2: green_led.on() 19 | if (x&4)==0: blue_led.off() 20 | elif (x&4)==4: blue_led.on() 21 | if (x&8)==0: ir_led.off() 22 | elif (x&8)==8: ir_led.on() 23 | 24 | 25 | s1 = Servo(1) # P7 Motor 26 | s2 = Servo(2) # P8 Steering 27 | print (s1.calibration()) # show throttle servo calibration 28 | cruise_speed = 1500 # how fast should the car drive, range from 1000 to 2000 29 | steering_gain = 800 30 | sensor.reset() # Initialize the camera sensor. 31 | sensor.set_pixformat(sensor.GRAYSCALE) # or sensor.RGB565 32 | sensor.set_framesize(sensor.QQVGA) # or sensor.QVGA (or others) 33 | clock = time.clock() # Tracks FPS. 34 | while(True): 35 | led_control(2) 36 | s1.pulse_width(cruise_speed) # move forward at cruise speed 37 | # s1.speed(120) # alternative way to control speed 38 | pyb.delay(10) 39 | clock.tick() # Track elapsed milliseconds between snapshots(). 40 | img = sensor.snapshot() # Take a picture and return the image. 41 | img.find_edges(image.EDGE_CANNY, threshold=(40, 174)) # Find edges 42 | lines = img.find_lines(threshold=40) # Find lines. 43 | counter = 0 44 | totalslope = 0 45 | if lines: 46 | for l in lines: 47 | img.draw_line(l, color=(127)) # Draw lines 48 | if (l[2]-l[0]) != 0: # don't allow vertical lines (infinite slope) 49 | slope = 0 # reset slope 50 | slope = (l[3]-l[1])/(l[2]-l[0]) 51 | if slope <= -0.5 or slope >= 0.5: # ignore lines that are mostly horizontal (slope between -0.5 and 0.5) 52 | totalslope = totalslope + slope 53 | counter = counter + 1 54 | if counter != 0: 55 | steer_angle = 1/(totalslope/counter) # 1/(average slope), to compensate for the inverse slope curve 56 | print (steer_angle) 57 | s2.pulse_width(1500 - int(steer_angle*steering_gain)) # steer 58 | pyb.delay(10) 59 | -------------------------------------------------------------------------------- /rover2.py: -------------------------------------------------------------------------------- 1 | # Autonomous rover code by Chris Anderson, 3DR, DIY Robocars 2 | # Copyrigbt 2017 3 | # Released under a BSD 2.0 Open Source Licence 4 | 5 | import sensor, image, time, pyb 6 | from pyb import Servo 7 | from pyb import LED 8 | 9 | red_led = LED(1) 10 | green_led = LED(2) 11 | blue_led = LED(3) 12 | ir_led = LED(4) 13 | 14 | def led_control(x): 15 | if (x&1)==0: red_led.off() 16 | elif (x&1)==1: red_led.on() 17 | if (x&2)==0: green_led.off() 18 | elif (x&2)==2: green_led.on() 19 | if (x&4)==0: blue_led.off() 20 | elif (x&4)==4: blue_led.on() 21 | if (x&8)==0: ir_led.off() 22 | elif (x&8)==8: ir_led.on() 23 | 24 | 25 | s1 = Servo(1) # P7 Motor 26 | s2 = Servo(2) # P8 Steering 27 | print (s1.calibration()) # show throttle servo calibration 28 | cruise_speed = 0 # how fast should the car drive, range from 1000 to 2000 29 | steering_gain = 130 30 | sensor.reset() # Initialize the camera sensor. 31 | sensor.set_pixformat(sensor.GRAYSCALE) # or sensor.RGB565 32 | sensor.set_framesize(sensor.QQVGA) # or sensor.QVGA (or others) 33 | clock = time.clock() # Tracks FPS. 34 | #s1.pulse_width(2000) # initialize the motor controller with a high signal for a second 35 | #pyb.delay(1000) 36 | #s1.pulse_width(1000) # return motor controller to low/off throttle 37 | while(True): 38 | led_control(2) 39 | s1.speed(120) 40 | pyb.delay(10) 41 | clock.tick() # Track elapsed milliseconds between snapshots(). 42 | img = sensor.snapshot() # Take a picture and return the image. 43 | img.find_edges(image.EDGE_CANNY, threshold=(40, 174)) # Find edges 44 | lines = img.find_lines(threshold=40) # Find lines. 45 | counter = 0 46 | totalslope = 0 47 | for l in lines: 48 | img.draw_line(l, color=(127)) # Draw lines 49 | if lines: 50 | if (l[2]-l[0]) != 0: # don't allow vertical lines (infinite slope) 51 | slope = 0 52 | slope = (l[3]-l[1])/(l[2]-l[0]) 53 | # print ('slope') 54 | # print (slope) 55 | if slope <= -0.5 or slope >= 0.5: # ignore lines that are mostly horizontal (slope between -0.5 and 0.5) 56 | totalslope = totalslope + slope 57 | counter = counter + 1 58 | if counter != 0: 59 | steer_angle = 1/(totalslope/counter) # 1/(average slope), to compensate for the inverse slope curve 60 | print (steer_angle) 61 | # s1.pulse_width(cruise_speed) # move forward at cruise speed 62 | s2.pulse_width(1500 - int(steer_angle*steering_gain)) # steer 63 | pyb.delay(10) 64 | --------------------------------------------------------------------------------