![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Simple Thruster test for T-100 / T-200 motors using a joystick.
main.cpp@0:b26782ea38a7, 2018-02-12 (annotated)
- Committer:
- roger_wee
- Date:
- Mon Feb 12 07:26:53 2018 +0000
- Revision:
- 0:b26782ea38a7
Simple Thruster test for T-100 motors
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
roger_wee | 0:b26782ea38a7 | 1 | #include "mbed.h" |
roger_wee | 0:b26782ea38a7 | 2 | #include "esc.h" |
roger_wee | 0:b26782ea38a7 | 3 | |
roger_wee | 0:b26782ea38a7 | 4 | DigitalOut my_led(LED1); |
roger_wee | 0:b26782ea38a7 | 5 | InterruptIn my_button(USER_BUTTON); |
roger_wee | 0:b26782ea38a7 | 6 | PwmOut m1(D3); |
roger_wee | 0:b26782ea38a7 | 7 | PwmOut m2(D4); |
roger_wee | 0:b26782ea38a7 | 8 | PwmOut m3(D5); |
roger_wee | 0:b26782ea38a7 | 9 | PwmOut m4(D6); |
roger_wee | 0:b26782ea38a7 | 10 | |
roger_wee | 0:b26782ea38a7 | 11 | |
roger_wee | 0:b26782ea38a7 | 12 | PwmOut m5(D7); |
roger_wee | 0:b26782ea38a7 | 13 | PwmOut m6(D8); |
roger_wee | 0:b26782ea38a7 | 14 | PwmOut m7(D9); |
roger_wee | 0:b26782ea38a7 | 15 | |
roger_wee | 0:b26782ea38a7 | 16 | //pwm values outputted to individual motors |
roger_wee | 0:b26782ea38a7 | 17 | int pwm = 0; |
roger_wee | 0:b26782ea38a7 | 18 | |
roger_wee | 0:b26782ea38a7 | 19 | //For serial display on pc |
roger_wee | 0:b26782ea38a7 | 20 | Serial pc(USBTX, USBRX); |
roger_wee | 0:b26782ea38a7 | 21 | |
roger_wee | 0:b26782ea38a7 | 22 | // Serial communication between Arduino NANO |
roger_wee | 0:b26782ea38a7 | 23 | RawSerial device(PA_11, PA_12); //TX RX |
roger_wee | 0:b26782ea38a7 | 24 | |
roger_wee | 0:b26782ea38a7 | 25 | int depth = 1; |
roger_wee | 0:b26782ea38a7 | 26 | |
roger_wee | 0:b26782ea38a7 | 27 | //maps value from incoming analog signal to correct range |
roger_wee | 0:b26782ea38a7 | 28 | //to be sent out to as pwm signal to motors |
roger_wee | 0:b26782ea38a7 | 29 | float map(float x, float in_min, float in_max, float out_min, float out_max) |
roger_wee | 0:b26782ea38a7 | 30 | { |
roger_wee | 0:b26782ea38a7 | 31 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
roger_wee | 0:b26782ea38a7 | 32 | } |
roger_wee | 0:b26782ea38a7 | 33 | |
roger_wee | 0:b26782ea38a7 | 34 | //declare analog input pin |
roger_wee | 0:b26782ea38a7 | 35 | AnalogIn joystickUD(A0); |
roger_wee | 0:b26782ea38a7 | 36 | |
roger_wee | 0:b26782ea38a7 | 37 | enum testStates {init, readInput, freeze, holdFreeze, holdUnfreeze} testState; |
roger_wee | 0:b26782ea38a7 | 38 | |
roger_wee | 0:b26782ea38a7 | 39 | void testTask(){ |
roger_wee | 0:b26782ea38a7 | 40 | //Actions |
roger_wee | 0:b26782ea38a7 | 41 | switch(testState) |
roger_wee | 0:b26782ea38a7 | 42 | { |
roger_wee | 0:b26782ea38a7 | 43 | case init: |
roger_wee | 0:b26782ea38a7 | 44 | break; |
roger_wee | 0:b26782ea38a7 | 45 | |
roger_wee | 0:b26782ea38a7 | 46 | case readInput: |
roger_wee | 0:b26782ea38a7 | 47 | //map raw joystick value |
roger_wee | 0:b26782ea38a7 | 48 | pwm = map(joystickUD.read(), 0.003, 0.996, 1200, 1800); |
roger_wee | 0:b26782ea38a7 | 49 | |
roger_wee | 0:b26782ea38a7 | 50 | |
roger_wee | 0:b26782ea38a7 | 51 | |
roger_wee | 0:b26782ea38a7 | 52 | //print mapped joystick values |
roger_wee | 0:b26782ea38a7 | 53 | // pc.printf("Pulse frequency: %d \n", pwm); |
roger_wee | 0:b26782ea38a7 | 54 | |
roger_wee | 0:b26782ea38a7 | 55 | //send pulse in microseconds to motors |
roger_wee | 0:b26782ea38a7 | 56 | m1.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 57 | m2.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 58 | m3.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 59 | m4.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 60 | |
roger_wee | 0:b26782ea38a7 | 61 | m5.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 62 | m6.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 63 | m7.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 64 | break; |
roger_wee | 0:b26782ea38a7 | 65 | |
roger_wee | 0:b26782ea38a7 | 66 | case freeze: |
roger_wee | 0:b26782ea38a7 | 67 | //send pulse in microseconds to motors |
roger_wee | 0:b26782ea38a7 | 68 | m1.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 69 | m2.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 70 | m3.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 71 | m4.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 72 | |
roger_wee | 0:b26782ea38a7 | 73 | m5.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 74 | m6.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 75 | m7.pulsewidth_us(pwm); |
roger_wee | 0:b26782ea38a7 | 76 | break; |
roger_wee | 0:b26782ea38a7 | 77 | |
roger_wee | 0:b26782ea38a7 | 78 | case holdUnfreeze: |
roger_wee | 0:b26782ea38a7 | 79 | break; |
roger_wee | 0:b26782ea38a7 | 80 | |
roger_wee | 0:b26782ea38a7 | 81 | case holdFreeze: |
roger_wee | 0:b26782ea38a7 | 82 | break; |
roger_wee | 0:b26782ea38a7 | 83 | |
roger_wee | 0:b26782ea38a7 | 84 | default: |
roger_wee | 0:b26782ea38a7 | 85 | break; |
roger_wee | 0:b26782ea38a7 | 86 | } |
roger_wee | 0:b26782ea38a7 | 87 | |
roger_wee | 0:b26782ea38a7 | 88 | //Transitions |
roger_wee | 0:b26782ea38a7 | 89 | switch(testState) |
roger_wee | 0:b26782ea38a7 | 90 | { |
roger_wee | 0:b26782ea38a7 | 91 | case init: |
roger_wee | 0:b26782ea38a7 | 92 | testState = readInput; |
roger_wee | 0:b26782ea38a7 | 93 | break; |
roger_wee | 0:b26782ea38a7 | 94 | |
roger_wee | 0:b26782ea38a7 | 95 | case readInput: |
roger_wee | 0:b26782ea38a7 | 96 | if (!my_button)//Button Pressed |
roger_wee | 0:b26782ea38a7 | 97 | { |
roger_wee | 0:b26782ea38a7 | 98 | testState = holdFreeze; |
roger_wee | 0:b26782ea38a7 | 99 | } |
roger_wee | 0:b26782ea38a7 | 100 | else |
roger_wee | 0:b26782ea38a7 | 101 | { |
roger_wee | 0:b26782ea38a7 | 102 | testState = readInput; |
roger_wee | 0:b26782ea38a7 | 103 | } |
roger_wee | 0:b26782ea38a7 | 104 | break; |
roger_wee | 0:b26782ea38a7 | 105 | |
roger_wee | 0:b26782ea38a7 | 106 | case freeze: |
roger_wee | 0:b26782ea38a7 | 107 | if (!my_button)//Button Pressed |
roger_wee | 0:b26782ea38a7 | 108 | { |
roger_wee | 0:b26782ea38a7 | 109 | testState = holdUnfreeze; |
roger_wee | 0:b26782ea38a7 | 110 | } |
roger_wee | 0:b26782ea38a7 | 111 | else //ButtonReleased |
roger_wee | 0:b26782ea38a7 | 112 | { |
roger_wee | 0:b26782ea38a7 | 113 | testState = freeze; |
roger_wee | 0:b26782ea38a7 | 114 | } |
roger_wee | 0:b26782ea38a7 | 115 | break; |
roger_wee | 0:b26782ea38a7 | 116 | |
roger_wee | 0:b26782ea38a7 | 117 | case holdFreeze: |
roger_wee | 0:b26782ea38a7 | 118 | if (my_button)//Button Released |
roger_wee | 0:b26782ea38a7 | 119 | { |
roger_wee | 0:b26782ea38a7 | 120 | testState = freeze; |
roger_wee | 0:b26782ea38a7 | 121 | } |
roger_wee | 0:b26782ea38a7 | 122 | else //Button StillPressed |
roger_wee | 0:b26782ea38a7 | 123 | { |
roger_wee | 0:b26782ea38a7 | 124 | testState = holdFreeze; |
roger_wee | 0:b26782ea38a7 | 125 | } |
roger_wee | 0:b26782ea38a7 | 126 | break; |
roger_wee | 0:b26782ea38a7 | 127 | |
roger_wee | 0:b26782ea38a7 | 128 | case holdUnfreeze: |
roger_wee | 0:b26782ea38a7 | 129 | if (my_button)//Button Released |
roger_wee | 0:b26782ea38a7 | 130 | { |
roger_wee | 0:b26782ea38a7 | 131 | testState = readInput; |
roger_wee | 0:b26782ea38a7 | 132 | } |
roger_wee | 0:b26782ea38a7 | 133 | else //Button StillPressed |
roger_wee | 0:b26782ea38a7 | 134 | { |
roger_wee | 0:b26782ea38a7 | 135 | testState = holdUnfreeze; |
roger_wee | 0:b26782ea38a7 | 136 | } |
roger_wee | 0:b26782ea38a7 | 137 | break; |
roger_wee | 0:b26782ea38a7 | 138 | |
roger_wee | 0:b26782ea38a7 | 139 | default: |
roger_wee | 0:b26782ea38a7 | 140 | break; |
roger_wee | 0:b26782ea38a7 | 141 | } |
roger_wee | 0:b26782ea38a7 | 142 | } |
roger_wee | 0:b26782ea38a7 | 143 | |
roger_wee | 0:b26782ea38a7 | 144 | |
roger_wee | 0:b26782ea38a7 | 145 | |
roger_wee | 0:b26782ea38a7 | 146 | int main() |
roger_wee | 0:b26782ea38a7 | 147 | { |
roger_wee | 0:b26782ea38a7 | 148 | testState = init; |
roger_wee | 0:b26782ea38a7 | 149 | my_led = 0; |
roger_wee | 0:b26782ea38a7 | 150 | |
roger_wee | 0:b26782ea38a7 | 151 | while (1) { |
roger_wee | 0:b26782ea38a7 | 152 | |
roger_wee | 0:b26782ea38a7 | 153 | testTask(); |
roger_wee | 0:b26782ea38a7 | 154 | // Read pressure sensor data if available |
roger_wee | 0:b26782ea38a7 | 155 | if (device.readable()) |
roger_wee | 0:b26782ea38a7 | 156 | { |
roger_wee | 0:b26782ea38a7 | 157 | // Receive depth in inches as an integer |
roger_wee | 0:b26782ea38a7 | 158 | depth = device.getc(); |
roger_wee | 0:b26782ea38a7 | 159 | |
roger_wee | 0:b26782ea38a7 | 160 | // Convert to feet |
roger_wee | 0:b26782ea38a7 | 161 | |
roger_wee | 0:b26782ea38a7 | 162 | // Display received data |
roger_wee | 0:b26782ea38a7 | 163 | pc.printf("Depth: %d \n", depth); |
roger_wee | 0:b26782ea38a7 | 164 | } |
roger_wee | 0:b26782ea38a7 | 165 | wait(0.01); // 10 ms |
roger_wee | 0:b26782ea38a7 | 166 | my_led =!my_led; |
roger_wee | 0:b26782ea38a7 | 167 | } |
roger_wee | 0:b26782ea38a7 | 168 | } |